卡尔曼滤波流程
- 状态的一步预测
X ( k + 1 ∣ k ) = F ( k ) X ( k ∣ k ) X(k+1|k)=F(k)X(k|k) X(k+1∣k)=F(k)X(k∣k) - 量测的预测
Z ( k + 1 ∣ k ) = H ( k ) X ( k + 1 ∣ k ) Z(k+1|k)=H(k)X(k+1|k) Z(k+1∣k)=H(k)X(k+1∣k) - 协方差一步预测
P ( k + 1 ∣ k ) = F ( k ) P ( k ∣ k ) F ( k ) T + Γ ( k ) Q ( k ) Γ T ( k ) P(k+1|k)=F(k)P(k|k)F(k)^{T}+\Gamma (k)Q(k)\Gamma^{T} (k) P(k+1∣k)=F(k)P(k∣k)F(k)T+Γ(k)Q(k)ΓT(k) - 新息协方差
S ( k + 1 ) = H ( k + 1 ) P ( k + 1 ∣ k ) H T ( k + 1 ) + R ( k + 1 ) S(k+1)=H(k+1)P(k+1|k)H^{T}(k+1)+R(k+1) S(k+1)=H(k+1)P(k+1∣k)HT(k+1)+R(k+1) - 增益
K ( k + 1 ) = P ( k + 1 ∣ k ) H T ( k + 1 ) S − 1 ( k + 1 ) K(k+1)=P(k+1|k)H^{T}(k+1)S^{-1}(k+1) K(k+1)=P(k+1∣k)HT(k+1)S−1(k+1) - 协方差更新方程
P ( k + 1 ∣ k + 1 ) = P ( k + 1 ∣ k ) − K ( k + 1 ) S ( k + 1 ) K T ( k + 1 ) P(k+1|k+1)=P(k+1|k)-K(k+1)S(k+1)K^{T}(k+1) P(k+1∣k+1)=P(k+1∣k)−K(k+1)S(k+1)KT(k+1) - 获取量测值