The meaning of the five core formulas in the Kalman filter algorithm
The Kalman filter algorithm that is often used in SLAM, here is a brief record of the five core formulas of the Kalman filter algorithm for subsequent inquiries. The derivation of the formula will not be repeated, and the five core formulas of the Kalman filter algorithm will be released directly:
1. State update equation
x k ^ = F k x ^ k − 1 + B k u k \hat{x_k}={F_k}\hat{x}_{k-1}+{B_k}{u_k} xk^=Fkx^k−1+Bkuk
P k = F k P k − 1 F k T + Q k {P_k}={F_k}{P_{k-1}}{F_k}^T+{Qk} Pk=FkPk−1FkT+Qk
2. Measurement update equation
x ^ ′ = x k ^ + K ( z k − H k x k ^ ) \hat{x}'=\hat{x_k}+K({z_k}-{H_k}\hat{x_k}) x^′=xk^+K ( zk−Hkxk^)
P k ′ = P k − K H k P k {P_k}'={P_k}-K{H_k}{P_k} Pk′=Pk−KHkPk
K = P k H k T ( H k P k H k T + R K ) − 1 {K}={P_k}{H_k}^T ({H_k}{P_k}{H_k}^T+{R_K})^{-1} K=PkHkT(HkPkHkT+RK)−1
Fk表示状态转移矩阵，描述系统的状态方程模型；
Bk表示状态的控制矩阵，描述系统的状态控制变化；
uk 为模型的修正向量，用于对建立模型的修正，该项在卡尔曼滤波算法中不是必备的；
Qk为过程噪声，描述了建立系统的模型准确度
Pk为协方差矩阵，描述了各状态量之间的相关性
P’k为经过修正的协方差矩阵
x^'k为经过量测方程修正的状态量估计值
K为卡尔曼增益，描述的是量测量对于状态量的修正权重
zk为观测量，多为传感器测量值或其等价值
Hk为量测矩阵，描述测量值与状态值之间的转换关系
Rk为量测噪声阵，描述传感器的测量噪声
整个算法的流程如下图所示：
流程的执行过程如下：
1.确定系统的状态转移矩阵Fk与量测矩阵Hk
2. 确定协方差矩阵初值P0与状态量初值 x0
3. 更新卡尔曼增益K
4. 根据测量向量zk 、卡尔曼增益KAnd measure uk , correct the state quantity, and get the state estimate value x^'k
of the update cycle 5. Update the covariance matrix P'k
6. According to the state transition matrix, recursive state equation, predict the next cycle state quantity x^k
7. According to the state transition matrix, recursive covariance matrix, predict the next cycle covariance matrix Pk