来看一下推导的思维流程:
- 卡尔曼滤波核心思想是推导的源头,也是推导的终点:
z k 估 ⃗ = z k 模 ⃗ + G k ∗ ( H − 1 ∗ y k 测 ⃗ − z k 模 ⃗ ) \begin{aligned}\vec{z_{k估}}&=\vec{z_{k模}}+\mathrm{G_k}*(\mathrm{H}^{-1}*\vec{y_{k测}}-\vec{z_{k模}})\end{aligned} zk估=zk模+Gk∗(H−1∗yk测−zk模) - z k 模 ⃗ = A ∗ z k − 1 估 ⃗ + B ∗ F k − 1 \vec{z_{k模}}=\mathrm{A}*\vec{z_{k-1估}}+\mathrm{B}*F_{k-1} zk模=A∗zk−1估+B∗Fk−1来自状态转移方程, y k 测 ⃗ \vec{y_{k测}} yk测是测量得到的。
- 推导出 G k = P k 模 P k 模 + ( H − 1 ) 2 R \mathrm{G_k}=\frac{\mathrm{P_{k模}}}{\mathrm{P_{k模}}+(\mathrm{H}^{-1})^2\mathrm{R}} Gk=Pk模+(H−1)2RPk模
- 推导出 P k 模 = A P k − 1 估 A T + Q \mathrm{P_{k模}}=\mathrm{A}\mathrm{P_{k-1估}}\mathrm{A}^T+\mathrm{Q} Pk模=APk−1估AT+Q
- 推导出 P k − 1 估 = ( I − G k − 1 ) P k − 1 模 \mathrm{P_{k-1估}}=(\mathrm{I}-\mathrm{G_{k-1}})\mathrm{P_{k-1模}} Pk−1估=(I−Gk−1)Pk−1模
观察上述5个公式,可以发现是一个迭代计算的过程,把与模型相关的公式放在前面,迭代计算过程为:
- z k 模 ⃗ = A ∗ z k − 1 估 ⃗ + B ∗ F k − 1 \vec{z_{k模}}=\mathrm{A}*\vec{z_{k-1估}}+\mathrm{B}*F_{k-1} zk模=A∗zk−1估+B∗Fk−1
- P k 模 = A P k − 1 估 A T + Q \mathrm{P_{k模}}=\mathrm{A}\mathrm{P_{k-1估}}\mathrm{A}^T+\mathrm{Q} Pk模=APk−1估AT+Q
- G k = P k 模 P k 模 + ( H − 1 ) 2 R \mathrm{G_k}=\frac{\mathrm{P_{k模}}}{\mathrm{P_{k模}}+(\mathrm{H}^{-1})^2\mathrm{R}} Gk=Pk模+(H−1)2RPk模
- z k 估 ⃗ = z k 模 ⃗ + G k ∗ ( H − 1 ∗ y k 测 ⃗ − z k 模 ⃗ ) \vec{z_{k估}}=\vec{z_{k模}}+\mathrm{G_k}*(\mathrm{H}^{-1}*\vec{y_{k测}}-\vec{z_{k模}}) zk估=zk模+Gk∗(H−1∗yk测−zk模)
- P k 估 = ( I − G k ) P k 模 \mathrm{P_{k估}}=(\mathrm{I}-\mathrm{G_k})\mathrm{P_{k模}} Pk估=(I−Gk)Pk模
卡尔曼的推导过程中将卡尔曼增益做了一个变换 G k = K k H \mathrm{G_k}=\mathrm{K_k}\mathrm{H} Gk=KkH, K k \mathrm{K_k} Kk是一个对角矩阵。我没想明白这样的有力理由,也许为了方便推导?不过结果是一样的,注意对角矩阵 H \mathrm{H} H左乘、右乘、转置是一样的, K k \mathrm{K_k} Kk形式的5个公式是:
- z k 模 ⃗ = A ∗ z k − 1 估 ⃗ + B ∗ F k − 1 \vec{z_{k模}}=\mathrm{A}*\vec{z_{k-1估}}+\mathrm{B}*F_{k-1} zk模=A∗zk−1估+B∗Fk−1
- P k 模 = A P k − 1 估 A T + Q \mathrm{P_{k模}}=\mathrm{A}\mathrm{P_{k-1估}}\mathrm{A}^T+\mathrm{Q} Pk模=APk−1估AT+Q
- K k = P k 模 H T H P k 模 H T + R \mathrm{K_k}=\frac{\mathrm{P_{k模}}\mathrm{H}^T}{\mathrm{H}\mathrm{P_{k模}}\mathrm{H}^T+\mathrm{R}} Kk=HPk模HT+RPk模HT
- z k 估 ⃗ = z k 模 ⃗ + K k ∗ ( y k 测 ⃗ − H ∗ z k 模 ⃗ ) \vec{z_{k估}}=\vec{z_{k模}}+\mathrm{K_k}*(\vec{y_{k测}}-\mathrm{H}*\vec{z_{k模}}) zk估=zk模+Kk∗(yk测−H∗zk模)
- P k 估 = ( I − K k H ) P k 模 \mathrm{P_{k估}}=(\mathrm{I}-\mathrm{K_k}\mathrm{H})\mathrm{P_{k模}} Pk估=(I−KkH)Pk模
A \mathrm{A} A是状态转移矩阵,是人为建模给定的。
B \mathrm{B} B是控制矩阵,是人为建模给定的。
Q , R \mathrm{Q},\mathrm{R} Q,R分别是正态分布模型噪声和测量噪声的协方差矩阵,是人为假设的,这也是需要调参的点。
z k 估 ⃗ \vec{z_{k估}} zk估是最优估计值,是卡尔曼滤波的最终输出值。
z 0 估 ⃗ , P 0 估 \vec{z_{0估}},\mathrm{P_{0估}} z0估,P0估是人为设定的。