"""
Kalman Filter Parameters for the MOT Algorithm

Code Written by
==>> Kyuewang Lee
        from Seoul National University, South Korea
        (currently in Perception and Intelligence Laboratory)

        Contact: kyuewang5056@gmail.com

"""

import numpy as np

########################################
#    Image Coordinate Kalman Filter    #
########################################

# Motion Model [u, v, du, dv, w, h] (6x6)
A = np.float32([[1, 0, 1, 0, 0, 0],
                [0, 1, 0, 1, 0, 0],
                [0, 0, 1, 0, 0, 0],
                [0, 0, 0, 1, 0, 0],
                [0, 0, 0, 0, 1, 0],
                [0, 0, 0, 0, 0, 1]])

# Error Covariance Matrix (6x6)
P = np.float32([[1, 0, 0, 0, 0, 0],
                [0, 1, 0, 0, 0, 0],
                [0, 0, 1, 0, 0, 0],
                [0, 0, 0, 1, 0, 0],
                [0, 0, 0, 0, 1, 0],
                [0, 0, 0, 0, 0, 1]]) * 1e-3
P = np.multiply(P, P)

# Unit Transformation Matrix (6x6)
H = np.float32([[1, 0, 0, 0, 0, 0],
                [0, 1, 0, 0, 0, 0],
                [0, 0, 1, 0, 0, 0],
                [0, 0, 0, 1, 0, 0],
                [0, 0, 0, 0, 1, 0],
                [0, 0, 0, 0, 0, 1]])

# State Covariance Matrix (6x6)
Q = np.float32([[1, 0, 0, 0, 0, 0],
                [0, 1, 0, 0, 0, 0],
                [0, 0, 1, 0, 0, 0],
                [0, 0, 0, 1, 0, 0],
                [0, 0, 0, 0, 1, 0],
                [0, 0, 0, 0, 0, 1]]) * 1e-3
Q = np.multiply(Q, Q) * 1e-1

# Measurement Covariance Matrix (6x6)
R = np.float32([[1, 0, 0, 0, 0, 0],
                [0, 1, 0, 0, 0, 0],
                [0, 0, 1, 0, 0, 0],
                [0, 0, 0, 1, 0, 0],
                [0, 0, 0, 0, 1, 0],
                [0, 0, 0, 0, 0, 1]]) * 1e-3
R = np.multiply(R, R)

# Kalman Gain Initialization (6x6)
K = np.float32([[1, 0, 0, 0, 0, 0],
                [0, 1, 0, 0, 0, 0],
                [0, 0, 1, 0, 0, 0],
                [0, 0, 0, 1, 0, 0],
                [0, 0, 0, 0, 1, 0],
                [0, 0, 0, 0, 0, 1]])

#########################################
#    Camera Coordinate Kalman Filter    #
#########################################
# Motion Model [X, Y, Z, dX, dY, dZ] (6x6)
cam_A = np.float32([[1, 0, 0, 1, 0, 0],
                    [0, 1, 0, 0, 1, 0],
                    [0, 0, 1, 0, 0, 1],
                    [0, 0, 0, 1, 0, 0],
                    [0, 0, 0, 0, 1, 0],
                    [0, 0, 0, 0, 0, 1]])

# Error Covariance Matrix (6x6)
cam_P = np.float32([[1, 0, 0, 0, 0, 0],
                    [0, 1, 0, 0, 0, 0],
                    [0, 0, 1, 0, 0, 0],
                    [0, 0, 0, 1, 0, 0],
                    [0, 0, 0, 0, 1, 0],
                    [0, 0, 0, 0, 0, 1]]) * 1e-3
cam_P = np.multiply(cam_P, cam_P)

# Unit Transformation Matrix (6x6)
cam_H = np.float32([[1, 0, 0, 0, 0, 0],
                    [0, 1, 0, 0, 0, 0],
                    [0, 0, 1, 0, 0, 0],
                    [0, 0, 0, 1, 0, 0],
                    [0, 0, 0, 0, 1, 0],
                    [0, 0, 0, 0, 0, 1]])

# State Covariance Matrix (6x6)
cam_Q = np.float32([[1, 0, 0, 0, 0, 0],
                    [0, 1, 0, 0, 0, 0],
                    [0, 0, 1, 0, 0, 0],
                    [0, 0, 0, 1, 0, 0],
                    [0, 0, 0, 0, 1, 0],
                    [0, 0, 0, 0, 0, 1]]) * 1e-2
cam_Q = np.multiply(cam_Q, cam_Q)

# Measurement Covariance Matrix (6x6)
cam_R = np.float32([[1, 0, 0, 0, 0, 0],
                    [0, 1, 0, 0, 0, 0],
                    [0, 0, 1, 0, 0, 0],
                    [0, 0, 0, 1, 0, 0],
                    [0, 0, 0, 0, 1, 0],
                    [0, 0, 0, 0, 0, 1]]) * 1e-4
cam_R = np.multiply(cam_R, cam_R)

