卡尔曼滤波的推导过程

3.1 系统模型

卡尔曼滤波的基本框架由状态方程和观测方程构成：

1. 状态方程：

   xk=Fkxk−1+Bkuk+wkxk​=Fk​xk−1​+Bk​uk​+wk​

   其中：

   • xkxk​：在时间 kk 的状态向量。
   • FkFk​：状态转移矩阵。
   • BkBk​：控制输入模型。
   • ukuk​：控制输入（外部影响）。
   • wkwk​：过程噪声，假设为高斯分布 wk∼N(0,Qk)wk​∼N(0,Qk​)。

2. 观测方程：

   zk=Hkxk+vkzk​=Hk​xk​+vk​

   其中：

   • zkzk​：在时间 kk 的观测向量。
   • HkHk​：观测矩阵。
   • vkvk​：观测噪声，假设为高斯分布 vk∼N(0,Rk)vk​∼N(0,Rk​)。

3.2 预测步骤

在每个时间步，首先进行预测：

1. 状态预测：

   x^k∣k−1=Fkx^k−1∣k−1+Bkukx^k∣k−1​=Fk​x^k−1∣k−1​+Bk​uk​

2. 协方差预测：

   Pk∣k−1=FkPk−1∣k−1FkT+QkPk∣k−1​=Fk​Pk−1∣k−1​FkT​+Qk​

3.3 更新步骤

一旦获得观测值 zkzk​，就执行更新步骤：

1. 计算卡尔曼增益：

   Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1Kk​=Pk∣k−1​HkT​(Hk​Pk∣k−1​HkT​+Rk​)−1

2. 更新状态估计：

   x^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)x^k∣k​=x^k∣k−1​+Kk​(zk​−Hk​x^k∣k−1​)

3. 更新协方差：

   Pk∣k=(I−KkHk)Pk∣k−1Pk∣k​=(I−Kk​Hk​)Pk∣k−1​

3.4 公式的解释

• 状态预测：根据上一个时刻的状态和控制输入来预测当前时刻的状态。
• 协方差预测：预估当前状态的不确定性，考虑过程噪声的影响。
• 卡尔曼增益：平衡预测和观测之间的权重，增益越高，表示观测对估计的影响越大。
• 状态更新：通过观测值修正预测的状态。
• 协方差更新：更新后的协方差表示新的不确定性。