卡尔曼滤波五个公式推导过程
一、假设条件
- 不确定性：所有状态量服从高斯分布，每个状态量的高斯分布有均值和方差，方差代表不确定性；
- 相关性：用协方差矩阵描述状态量间的不确定关系，一个变量可能影响其他变量。为对称矩阵，其非对角线元素代表变量之间的相关程度。有如下性质
( 1 )
二、基本定义
假设目标估计是机器人的位置和速度（两个融合数据源），则状态量可表示为：
( 2 )
记k时刻：；状态量协方差矩阵：
；
三、状态预测过程
已知模型为：
( 3 )
表示成矩阵形式为：
( 4 )
其中Fk为状态转移矩阵，根据已知模型，描述k-1时刻状态预测得到k时刻状态的过程。
的协方差矩阵为：
考虑外部控制因素时，已知模型变为：
( 5 )
其矩阵形式为：
( 6 )
其中为控制矩阵，
为控制向量。
考虑每一步预测必然存在的预测过程噪声，用过程噪声协方差矩阵描述其产生的不确定度，因此卡尔曼滤波状态预测方程为：
( 7 )
|
参数 |
含义 |
| |
k 时刻的先验状态估计值，根据k-1时刻的最优估计预测的k时刻的结果，是预测方程的结果，滤波的中间计算结果 |
|
k 时刻的先验估计协方差（的协方差），滤波的中间计算结果 |
|
|
k-1时刻的后验状态估计值，即更新结果，滤波的结果之一 |
|
|
k-1时刻的后验估计协方差（即的协方差），滤波的结果之一 |
四、状态观测更新过程
实际过程中，状态量里的元素并不是都可直接测量，因此利用观测转移矩阵表示观测结果预测值和状态量之间的转换：
( 8 )
其观测结果预测过程协方差矩阵为：
( 9 )
多个传感器实际观测结果组成的向量为观测向量，同时考虑观测过程中传感器的测量噪声，用观测噪声协方差矩阵
描述传感器间的不确定度。
目前，观测过程得到两个高斯分布，一个是预测所得，一个是传感器测量得到，即：
观测预测分布： ( 10 )
观测过程分布： ( 11 )
两个高斯分布相乘后的结果还符合高斯分布，组合成了一个新的高斯分布，即：
( 12 )
令，则新高斯分布：
( 13 )
其矩阵形式为：
( 14 )
结合式(10)，(11)和(14)，得卡尔曼滤波全公式：
( 15 )
其中，为卡尔曼增益。
将式(15-b)方程等号两边均左乘；式(15-c)方程等号两边均左乘
和右乘
，得卡尔曼滤波化简公式：
( 16 )
|
参数 |
含义 |
| |
k时刻的后验状态估计值或更新结果，滤波的结果之一 |
|
实际观测和预测观测的残差，和 |
|
|
|
k时刻的后验估计协方差（即的协方差），滤波的结果之一 |
因此，式(16)得到的就是状态观测更新过程方程，经过该方程得到的状态为k时刻的最佳预测，状态最优估计。