"""
# 卡尔曼滤波分为两个阶段：预测阶段 和 更新阶段
预测阶段：根据上一时刻(t-1时刻)的后验估计值来估计当前时刻(t时刻)的先验估计值
    Eq1. 状态预测公式:     x^~(t) = F*x^(t-1) + B*u(t-1)  + w(t-1) 
                         或简化 x^~(t) = F*x^(t-1)
    Eq2. 协方差预测公式:   P~(t) = F*P(t-1)*F.T + Q
更新阶段：使用当前时刻(t时刻)的测量值来更正预测阶段估计值，得到当前时刻(t时刻)的后验估计值
    Eq3. 卡尔曼增益:      K(t) = P~(t)*H.T*(H*P~(t)*H.T + R).I
    Eq4. 状态更新公式:    x^(t) = x^(t-1) + K(t)*(z(t) - H*x^~(t))
    Eq5. 协方差更新公式:   P(t) = (I - K(t)*H)*P~(t)
"""
class KalmanFilter(object):
    def __init__(self):
        pass
    
    # 状态、协方差 向测量空间映射
    def project(self, mean, covariance):
        std = [self._std_weight_position * mean[3],
               self._std_weight_position * mean[3],
               1e-1,
               self._std_weight_position * mean[3]]
        # 初始化测量过程中噪声矩阵 R, 即 innovation_cov, 该噪声矩阵与检测框的高相关 ，它是一个4x4的对角矩阵
        innovation_cov = np.diag(np.square(std))

        # Eq4. 状态更新公式: x^(t) = x^(t-1) + K(t)*(z(t) - H*x^~(t))
        # Eq4. 中将 状态向量 映射到 测量空间 H*x^~(t)
        mean = np.dot(self._update_mat, mean)
        # Eq3. 卡尔曼增益: K(t) = P~(t) * H.T * (H * P~(t) * H.T + R).I
        # Eq3. 中将协方差矩阵 P~(t) 映射到测量空间 H * P~(t) * H.T
        covariance = np.linalg.multi_dot((self._update_mat, covariance, self._update_mat.T))

        return mean, covariance + innovation_cov  # H * x^(t)； H * P~(t) * H.T + R
    
    # 马氏距离
    def gating_distance(self, mean, covariance, measurements, only_position=False):
        mean, covariance = self.project(mean, covariance)
        if only_position:
            mean, covariance = mean[:2], covariance[:2, :2]
            measurements = measurements[:, :2]
        
        # 将 埃尔米特（实数即对称）正定的矩阵 分解为下/上三角矩阵
        cholesky_factor = np.linalg.cholesky(covariance)
        d = measurements - mean  # Xi - Xj
        # scipy.linalg.solve_triangular解下三角线性方程组 a*x = b 方程中的 x，（假定a是一个上/下三角矩阵）
        z = scipy.linalg.solve_triangular(cholesky_factor, d.T, lower=True, check_finite=False, 			                                           overwrite_b=True)
        squared_maha = np.sum(z * z, axis=0)  # Dmaha**2 = z.T * z = z**2, 见等价推理

        return squared_maha