卡尔曼滤波的推导过程
3.1 系统模型
卡尔曼滤波的基本框架由状态方程和观测方程构成：
-
状态方程：
xk=Fkxk−1+Bkuk+wkxk=Fkxk−1+Bkuk+wk
其中：
- xkxk：在时间 kk 的状态向量。
- FkFk：状态转移矩阵。
- BkBk：控制输入模型。
- ukuk：控制输入（外部影响）。
- wkwk：过程噪声，假设为高斯分布 wk∼N(0,Qk)wk∼N(0,Qk)。
-
观测方程：
zk=Hkxk+vkzk=Hkxk+vk
其中：
- zkzk：在时间 kk 的观测向量。
- HkHk：观测矩阵。
- vkvk：观测噪声，假设为高斯分布 vk∼N(0,Rk)vk∼N(0,Rk)。
3.2 预测步骤
在每个时间步，首先进行预测：
-
状态预测：
x^k∣k−1=Fkx^k−1∣k−1+Bkukx^k∣k−1=Fkx^k−1∣k−1+Bkuk
-
协方差预测：
Pk∣k−1=FkPk−1∣k−1FkT+QkPk∣k−1=FkPk−1∣k−1FkT+Qk
3.3 更新步骤
一旦获得观测值 zkzk，就执行更新步骤：
-
计算卡尔曼增益：
Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1
-
更新状态估计：
x^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)x^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)
-
更新协方差：
Pk∣k=(I−KkHk)Pk∣k−1Pk∣k=(I−KkHk)Pk∣k−1
3.4 公式的解释
- 状态预测：根据上一个时刻的状态和控制输入来预测当前时刻的状态。
- 协方差预测：预估当前状态的不确定性，考虑过程噪声的影响。
- 卡尔曼增益：平衡预测和观测之间的权重，增益越高，表示观测对估计的影响越大。
- 状态更新：通过观测值修正预测的状态。
- 协方差更新：更新后的协方差表示新的不确定性。
