卡尔曼滤波的完整流程
卡尔曼滤波的完整流程
flyfish
1. 初始化
在第一个时刻，需要初始化状态向量和误差协方差矩阵：
- 状态向量（State Vector） x 0 \mathbf{x}_0 x0：对系统初始状态的估计。
- 误差协方差矩阵（Error Covariance Matrix） P 0 \mathbf{P}_0 P0：初始状态估计的不确定性。
2. 预测步骤（Prediction Step）
预测系统在下一个时刻的状态和误差协方差矩阵。
- 预测状态（Predicted State） x ^ k ∣ k − 1 \hat{\mathbf{x}}_{k|k-1} x^k∣k−1： x ^ k ∣ k − 1 = A k − 1 x ^ k − 1 ∣ k − 1 + B k − 1 u k − 1 \hat{\mathbf{x}}_{k|k-1} = \mathbf{A}_{k-1} \hat{\mathbf{x}}_{k-1|k-1} + \mathbf{B}_{k-1} \mathbf{u}_{k-1} x^k∣k−1=Ak−1x^k−1∣k−1+Bk−1uk−1其中， A k − 1 \mathbf{A}_{k-1} Ak−1 是状态转移矩阵， B k − 1 \mathbf{B}_{k-1} Bk−1 是控制输入矩阵， u k − 1 \mathbf{u}_{k-1} uk−1 是控制输入。
- 预测误差协方差矩阵（Predicted Error Covariance Matrix） P k ∣ k − 1 \mathbf{P}_{k|k-1} Pk∣k−1： P k ∣ k − 1 = A k − 1 P k − 1 ∣ k − 1 A k − 1 T + Q k − 1 \mathbf{P}_{k|k-1} = \mathbf{A}_{k-1} \mathbf{P}_{k-1|k-1} \mathbf{A}_{k-1}^T + \mathbf{Q}_{k-1} Pk∣k−1=Ak−1Pk−1∣k−1Ak−1T+Qk−1其中， Q k − 1 \mathbf{Q}_{k-1} Qk−1 是过程噪声协方差矩阵。
3. 更新步骤（Update Step）
利用新测量数据更新预测的状态和误差协方差矩阵。
- 卡尔曼增益（Kalman Gain） K k \mathbf{K}_k Kk： K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R k ) − 1 \mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k)^{-1} Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1其中， H k \mathbf{H}_k Hk 是观测矩阵， R k \mathbf{R}_k Rk 是观测噪声协方差矩阵。
- 更新状态（Updated State） x ^ k ∣ k \hat{\mathbf{x}}_{k|k} x^k∣k： x ^ k ∣ k = x ^ k ∣ k − 1 + K k ( z k − H k x ^ k ∣ k − 1 ) \hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1}) x^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)其中， z k \mathbf{z}_k zk 是测量向量。
- 更新误差协方差矩阵（Updated Error Covariance Matrix） P k ∣ k \mathbf{P}_{k|k} Pk∣k： P k ∣ k = ( I − K k H k ) P k ∣ k − 1 \mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1} Pk∣k=(I−KkHk)Pk∣k−1其中， I \mathbf{I} I 是单位矩阵。
4. 循环迭代
重复步骤2和步骤3，处理每个新的测量数据。
各个字母的含义
1. 预测步骤（Prediction Step）
预测步骤包括状态预测和误差协方差预测。
状态预测（State Prediction）
x ^ k ∣ k − 1 = A k − 1 x k − 1 + B k − 1 u k − 1 \hat{\mathbf{x}}_{k|k-1} = \mathbf{A}_{k-1} \mathbf{x}_{k-1} + \mathbf{B}_{k-1} \mathbf{u}_{k-1} x^k∣k−1=Ak−1xk−1+Bk−1uk−1
- x ^ k ∣ k − 1 \hat{\mathbf{x}}_{k|k-1} x^k∣k−1：预测的状态向量（在时间 k k k 时的先验估计）。
- A k − 1 \mathbf{A}_{k-1} Ak−1：状态转移矩阵，描述系统从时间 k − 1 k-1 k−1 到时间 k k k 的动态。
- x k − 1 \mathbf{x}_{k-1} xk−1：在时间 k − 1 k-1 k−1 时的状态向量（后验估计）。
- B k − 1 \mathbf{B}_{k-1} Bk−1：控制输入矩阵，描述控制输入对系统状态的影响。
- u k − 1 \mathbf{u}_{k-1} uk−1：在时间 k − 1 k-1 k−1 时的控制输入向量。
误差协方差预测（Error Covariance Prediction）
P k ∣ k − 1 = A k − 1 P k − 1 ∣ k − 1 A k − 1 T + Q k − 1 \mathbf{P}_{k|k-1} = \mathbf{A}_{k-1} \mathbf{P}_{k-1|k-1} \mathbf{A}_{k-1}^T + \mathbf{Q}_{k-1} Pk∣k−1=Ak−1Pk−1∣k−1Ak−1T+Qk−1
- P k ∣ k − 1 \mathbf{P}_{k|k-1} Pk∣k−1：预测的误差协方差矩阵（在时间 k k k 时的先验估计的不确定性）。
- P k − 1 ∣ k − 1 \mathbf{P}_{k-1|k-1} Pk−1∣k−1：在时间 k − 1 k-1 k−1 时的误差协方差矩阵（后验估计的不确定性）。
- A k − 1 \mathbf{A}_{k-1} Ak−1：状态转移矩阵。
- Q k − 1 \mathbf{Q}_{k-1} Qk−1：过程噪声协方差矩阵，描述系统过程噪声的不确定性。
2. 更新步骤（Update Step）
更新步骤包括计算卡尔曼增益、更新状态估计和更新误差协方差矩阵。
计算卡尔曼增益（Kalman Gain Calculation）
K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R k ) − 1 \mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k)^{-1} Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1
- K k \mathbf{K}_k Kk：卡尔曼增益矩阵，描述测量值对状态估计的影响。
- P k ∣ k − 1 \mathbf{P}_{k|k-1} Pk∣k−1：预测的误差协方差矩阵。
- H k \mathbf{H}_k Hk：观测矩阵，描述状态向量到测量向量的映射。
- R k \mathbf{R}_k Rk：测量噪声协方差矩阵，描述测量噪声的不确定性。
更新状态估计（State Update）
x k ∣ k = x ^ k ∣ k − 1 + K k ( z k − H k x ^ k ∣ k − 1 ) \mathbf{x}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1}) xk∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)
- x k ∣ k \mathbf{x}_{k|k} xk∣k：更新后的状态向量（在时间 k k k 时的后验估计）。
- x ^ k ∣ k − 1 \hat{\mathbf{x}}_{k|k-1} x^k∣k−1：预测的状态向量。
- K k \mathbf{K}_k Kk：卡尔曼增益矩阵。
- z k \mathbf{z}_k zk：在时间 k k k 时的实际测量向量。
- H k \mathbf{H}_k Hk：观测矩阵。
- H k x ^ k ∣ k − 1 \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1} Hkx^k∣k−1：预测测量值。
更新误差协方差矩阵（Error Covariance Update）
P k ∣ k = ( I − K k H k ) P k ∣ k − 1 \mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1} Pk∣k=(I−KkHk)Pk∣k−1
- P k ∣ k \mathbf{P}_{k|k} Pk∣k：更新后的误差协方差矩阵（在时间 k k k 时的后验估计的不确定性）。
- I \mathbf{I} I：单位矩阵。
- K k \mathbf{K}_k Kk：卡尔曼增益矩阵。
- H k \mathbf{H}_k Hk：观测矩阵。
- P k ∣ k − 1 \mathbf{P}_{k|k-1} Pk∣k−1：预测的误差协方差矩阵。