卡尔曼滤波算法中的五大核心公式含义
在SLAM中经常会用的卡尔曼滤波算法，这里简单的记录一下卡尔曼滤波算法的五大核心公式，以便后续查询，公式的推导不在作赘述，直接放出卡尔曼滤波算法的五大核心公式：
1. 状态更新方程
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.测量更新方程
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 、卡尔曼增益K以及量测量uk，修正状态量，得到该更新周期的状态估计值x^'k
5. 更新协方差矩阵P’k
6. 根据状态转移矩阵，递推状态方程，预测下一周期状态量 x^k
7.根据状态转移矩阵，递推协方差矩阵，预测下一周期协方差阵Pk