黄金五式
- 预测状态估计：
x ^ k ∣ k − 1 = F k x ^ k − 1 ∣ k − 1 + B k u k \hat{x}_{k|k-1} = F_k \hat{x}_{k-1|k-1} + B_k u_k x^k∣k−1=Fkx^k−1∣k−1+Bkuk
其中， x ^ k − 1 ∣ k − 1 \hat{x}_{k-1|k-1} x^k−1∣k−1代表在时刻 k − 1 k-1 k−1时对状态的最优估计， F k F_k Fk是状态转移矩阵， B k B_k Bk是控制矩阵， u k u_k uk是外部控制输入。
该公式用来预测下一个时刻的状态估计值，基于上一个时刻的状态估计值和控制输入进行计算，并且假设系统被建模成一个线性动态系统。
- 预测误差协方差：
P k ∣ k − 1 = F k P k − 1 ∣ k − 1 F k T + Q k P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k Pk∣k−1=FkPk−1∣k−1FkT+Qk
其中， P k − 1 ∣ k − 1 P_{k-1|k-1} Pk−1∣k−1是在时刻 k − 1 k-1 k−1时对状态误差的最优估计协方差矩阵， Q k Q_k Qk是过程噪声的协方差矩阵。
该公式用来计算预测状态估计的误差协方差矩阵，它包含系统变化和测量误差的影响。
- 更新卡尔曼增益：
K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R k ) − 1 K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1} Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1
其中， H k H_k Hk是观测矩阵， R k R_k Rk是观测噪声的协方差矩阵。
该公式用来计算卡尔曼增益，它反映了测量值和预测值之间的不确定性，是根据系统的动态模型和观测值计算出来的。
- 更新状态估计：
x ^ k ∣ k = x ^ k ∣ k − 1 + K k ( z k − H k x ^ k ∣ k − 1 ) \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k(z_k - H_k\hat{x}_{k|k-1}) x^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)
其中， z k z_k zk是在时刻 k k k的观测值。
该公式用来更新状态估计值，它基于预测状态估计值和测量值以及卡尔曼增益来计算。
- 更新误差协方差：
P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k|k} = (I - K_k H_k) P_{k|k-1} Pk∣k=(I−KkHk)Pk∣k−1
该公式用来更新误差协方差矩阵，它基于预测误差协方差矩阵和卡尔曼增益来计算。
卡尔曼滤波的黄金五式组合在一起，可以实现对系统状态的最优估计，同时也是卡尔曼滤波算法的核心部分。