卡尔曼滤波（Kalman Filter，KF）基础推导及其实例

已于 2023-08-13 11:31:05 修改
阅读量544 收藏 4
点赞数 2
分类专栏： 自动驾驶控制 文章标签： 概率论 算法 线性代数
于 2023-03-05 15:06:25 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/chaser30/article/details/129345651
版权
文章详细介绍了卡尔曼滤波的基本概念，包括卡尔曼滤波中涉及的先验估计、卡尔曼增益、后验估计和误差协方差矩阵的推导过程。通过数学公式展示了如何计算卡尔曼增益和更新后验估计值，以及在控制领域中的应用，解释了如何通过卡尔曼滤波来减小系统状态和测量数据中的噪声影响，提高估计精度。
摘要由CSDN通过智能技术生成

文章目录

卡尔曼滤波(Kalman Filter，KF)

自用笔记，带一些个人理解和知识拓展，详见b站Dr.CAN卡尔曼滤波专题，权侵删
本篇为卡尔曼滤波入门介绍，推导基于线性控制系统，但使用例子为测量硬币真实直径

涉及到的主要知识

  • 线性代数
  • 概率论
  • 矩阵论
  • 最优估计

卡尔曼滤波中涉及的几个常用概念

  • 先验估计：通过理论公式计算得到的，无干扰情况下理论的输出值
  • 卡尔曼增益：通过概率论知识，估计出对理论输出值的实际输出误差补偿
  • 后验估计：理论计算+综合考虑了干扰影响后的实际输出值（通过概率对误差进行估计，然后反馈补偿到理论输出值上）

卡尔曼滤波中最重要的五个公式

已知：实际控制系统中存在过程噪声 w w w和测量噪声 v v v，它们均符合高斯分布，分别写作
{ x k = A x k − 1 + w k − 1 z k = H x k + v k { w ∼ p ( 0 , Q ) v ∼ p ( 0 , R ) \begin{cases} x_k = Ax_{k-1}+w_{k-1}\\ z_k = Hx_k + v_k \end{cases}\\ \begin{cases} w \sim p(0,Q)\\ v \sim p(0,R) \end{cases} {xk​=Axk−1​+wk−1​zk​=Hxk​+vk​​{w∼p(0,Q)v∼p(0,R)​
式中 0 0 0表示两种噪声的期望值， Q Q Q和 R R R分别表示两种噪声分布的方差。
经过推导，得到以下五个重要公式
先验估计： x ^ k − = A x k − 1 先验误差协方差： P k − = A P k − 1 A T + Q 卡尔曼增益： K k = P k − H T H P k − H T + R 后验估计： x ^ k = x ^ k − 1 + K k ( z k − x ^ k − 1 ) 更新误差协方差： P k = ( I − K k H ) P k − \begin{split} 先验估计：\hat x_k^- &= Ax_{k-1}\\ 先验误差协方差：P_k^- &= AP_{k-1}A^T + Q\\ 卡尔曼增益：K_k &= \frac{P_k^-H^T}{HP_k^-H^T + R}\\ 后验估计：\hat x_k &= \hat x_{k-1}+K_k(z_k - \hat x_{k-1})\\ 更新误差协方差：P_k &= (I - K_kH)P_k^- \end{split} 先验估计：x^k−​先验误差协方差：Pk−​卡尔曼增益：Kk​后验估计：x^k​更新误差协方差：Pk​​=Axk−1​=APk−1​AT+Q=HPk−​HT+RPk−​HT​=x^k−1​+Kk​(zk​−x^k−1​)=(I−Kk​H)Pk−​​

卡尔曼增益的推导

已知两个相互独立事件的目标发生概率分别为 σ 1 \sigma_1 σ1​和 σ 2 \sigma_2 σ2​，求其联合概率 σ Z ^ \sigma_{\hat Z} σZ^​。求 K k K_k Kk​促使 σ Z ^ \sigma_{\hat Z} σZ^​的不确定性最小，即求方差 v a r ( Z ^ ) var(\hat Z) var(Z^)最小值，此时即为最优解，推导过程如下
σ Z ^ 2 = v a r [ Z 1 + K k ( Z 2 − Z 1 ) ] = v a r ( Z 1 − K k Z 1 + K k Z 2 ) = v a r [ ( 1 − K k ) Z 1 + K k Z 2 ] = v a r [ ( 1 − K k ) Z 1 ] + v a r ( K k Z 2 ) = ( 1 − K k ) 2 v a r ( Z 1 ) + K k 2 v a r ( Z 2 ) = ( 1 − K k ) 2 σ 1 2 + K k 2 σ 2 2 \begin{split} \sigma_{\hat Z}^2 &= var[Z_1+K_k(Z_2-Z_1)]\\ &=var(Z_1-K_kZ_1+K_kZ_2)\\ &=var[(1-K_k)Z_1+K_kZ_2]\\ &=var[(1-K_k)Z_1]+var(K_kZ_2)\\ &=(1-K_k)^2var(Z_1)+K_k^2var(Z_2)\\ &=(1-K_k)^2\sigma_1^2+K_k^2\sigma_2^2\\ \end{split} σZ^2​​=var[Z1​+Kk​(Z2​−Z1​)]=var(Z1​−Kk​Z1​+Kk​Z2​)=var[(1−Kk​)Z1​+Kk​Z2​]=var[(1−Kk​)Z1​]+var(Kk​Z2​)=(1−Kk​)2var(Z1​)+Kk2​var(Z2​)=(1−Kk​)2σ12​+Kk2​σ22​​
为了求解方差 v a r ( Z ^ ) var(\hat Z) var(Z^)最小值，对上式进行求导，找其零点，因此易得
δ σ Z ^ 2 δ K k = − 2 ( 1 − K k ) σ 1 2 + 2 K k σ 2 2 = 0 \frac{\delta\sigma_{\hat Z}^2}{\delta K_k} = -2(1-K_k)\sigma_1^2+2K_k\sigma_2^2=0\\ δKk​δσZ^2​​=−2(1−Kk​)σ12​+2Kk​σ22​=0
对其进行整理，有
− σ 1 2 + K k σ 1 2 + K k σ 1 2 = 0 K k ( σ 1 2 + σ 2 2 ) = σ 1 2 ⇒ K k = σ 1 2 σ 1 2 + σ 2 2 \begin{split} -\sigma_1^2+K_k\sigma_1^2+K_k\sigma_1^2&=0\\ K_k(\sigma_1^2+\sigma_2^2)&=\sigma_1^2\\ \Rightarrow K_k&=\frac{\sigma_1^2}{\sigma_1^2+\sigma_2^2} \end{split} −σ12​+Kk​σ12​+Kk​σ12​Kk​(σ12​+σ22​)⇒Kk​​=0=σ12​=σ12​+σ22​σ12​​​

后验估计表达式的推导

估计真实数据的通常做法为：取多次测量的平均值。根据该思想，写出测量k次时的估计真实值,表达式如下
x ^ k = 1 k ( z 1 + z 2 + . . . + z k ) = 1 k ( z 1 + z 2 + . . . + z k − 1 ) + 1 k z k = 1 k ⋅ k − 1 k − 1 ( z 1 + z 2 + . . . + z k − 1 ) + 1 k z k \begin{split} \hat x_k &= \frac 1k(z_1+z_2+...+z_k)\\ &= \frac 1k(z_1+z_2+...+z_{k-1})+\frac 1kz_k\\ &= \frac 1k·\frac{k-1}{k-1}(z_1+z_2+...+z_{k-1})+\frac 1kz_k\\ \end{split} x^k​​=k1​(z1​+z2​+...+zk​)=k1​(z1​+z2​+...+zk−1​)+k1​zk​=k1​⋅k−1k−1​(z1​+z2​+...+zk−1​)+k1​zk​​
上式中 ( z 1 + z 2 + . . . + z k − 1 ) / k − 1 (z_1+z_2+...+z_{k-1})/{k-1} (z1​+z2​+...+zk−1​)/k−1为第k-1次测量时的平均估计值 x ^ k − 1 \hat x_{k-1} x^k−1​，因此上式可改写为
x ^ k = k − 1 k x ^ k − 1 + 1 k z k = x ^ k − 1 − 1 k x ^ k − 1 + 1 k z k = x ^ k − 1 + 1 k ( z k − x ^ k − 1 ) \begin{split} \hat x_k &= \frac {k-1}{k}\hat x_{k-1}+\frac 1kz_k\\ &= \hat x_{k-1} - \frac 1k \hat x_{k-1} + \frac 1kz_k\\ &= \hat x_{k-1}+\frac 1k(z_k - \hat x_{k-1}) \end{split} x^k​​=kk−1​x^k−1​+k1​zk​=x^k−1​−k1​x^k−1​+k1​zk​=x^k−1​+k1​(zk​−x^k−1​)​
式中的 1 k \frac 1k k1​称为卡尔曼增益（卡尔曼系数），常写作 K k K_k Kk​。经过整理后即为尔曼滤波的核心公式之一，写作
x ^ k = x ^ k − 1 + K k ( z k − x ^ k − 1 ) \hat x_k= \hat x_{k-1}+K_k(z_k - \hat x_{k-1}) x^k​=x^k−1​+Kk​(zk​−x^k−1​)
该式的物理含义为：

当前的估计值 = 上一次的估计值 + 卡尔曼增益*(当前测量值-上一次的估计值)

误差协方差矩阵的推导

经过上面的推导，现在应用卡尔曼滤波时只剩下联合概率的先验值 P k − P_k^- Pk−​是未知的了，因此我们即将推导它的公式，已知联合概率的先验表达式如下
P k − = E ( e k − e k − T ) P_k^- = E(e_k^-e_k^{-T}) Pk−​=E(ek−​ek−T​)
由之前的推导已知
e k − = x k − − x ^ k − = A x k − 1 + B u k − 1 + w k − 1 − A x ^ k − 1 − B u k − 1 = A ( x k − 1 − x ^ k − ) + w k − 1 = A e k − 1 + w k − 1 \begin{split} e_k^- &= x_k^- - \hat x_k^-\\ &= Ax_{k-1} + Bu_{k-1} + w_{k-1} - A \hat x_{k-1} - Bu_{k-1}\\ &= A(x_{k-1} - \hat x_k^-) + w_{k-1}\\ &= Ae_{k-1} + w_{k-1} \end{split} ek−​​=xk−​−x^k−​=Axk−1​+Buk−1​+wk−1​−Ax^k−1​−Buk−1​=A(xk−1​−x^k−​)+wk−1​=Aek−1​+wk−1​​
将该结果代入原式后可得
P k − = E ( e k − e k − T ) = E [ ( A e k − 1 + w k − 1 ) ( A e k − 1 + w k − 1 ) T ] = E [ ( A e k − 1 + w k − 1 ) ( A e k − 1 ) T + ( w k − 1 ) T ] = E [ ( A e k − 1 + w k − 1 ) ( e k − 1 T A T + w k − 1 T ] = E ( A e k − 1 e k − 1 T A T + A e k − 1 w k − 1 T + w k − 1 e k − 1 T A T + w k − 1 w k − 1 T ) = E ( A e k − 1 e k − 1 T A T ) + E ( A e k − 1 w k − 1 T ) + E ( w k − 1 e k − 1 T A T ) + E ( w k − 1 w k − 1 T ) \begin{split} P_k^- &= E(e_k^-e_k^{-T})\\ &= E[(Ae_{k-1} + w_{k-1})(Ae_{k-1} + w_{k-1})^T]\\ &= E[(Ae_{k-1} + w_{k-1})(Ae_{k-1})^T + (w_{k-1})^T]\\ &= E[(Ae_{k-1} + w_{k-1})(e_{k-1}^TA^T + w_{k-1}^T]\\ &= E(Ae_{k-1}e_{k-1}^TA^T + Ae_{k-1}w_{k-1}^T + w_{k-1}e_{k-1}^TA^T + w_{k-1}w_{k-1}^T)\\ &= E(Ae_{k-1}e_{k-1}^TA^T) + E(Ae_{k-1}w_{k-1}^T) + E(w_{k-1}e_{k-1}^TA^T) + E(w_{k-1}w_{k-1}^T)\\ \end{split} Pk−​​=E(ek−​ek−T​)=E[(Aek−1​+wk−1​)(Aek−1​+wk−1​)T]=E[(Aek−1​+wk−1​)(Aek−1​)T+(wk−1​)T]=E[(Aek−1​+wk−1​)(ek−1T​AT+wk−1T​]=E(Aek−1​ek−1T​AT+Aek−1​wk−1T​+wk−1​ek−1T​AT+wk−1​wk−1T​)=E(Aek−1​ek−1T​AT)+E(Aek−1​wk−1T​)+E(wk−1​ek−1T​AT)+E(wk−1​wk−1T​)​
关注上式中的第二项与第三项，根据下式
e k − 1 = x k − 1 − x ^ k − 1 x k = A x k − 1 + B u k − 1 + w k − 1 e_{k-1} = x_{k-1} - \hat x_{k-1}\\ x_k = Ax_{k-1}+Bu_{k-1} + w_{k-1}\\ ek−1​=xk−1​−x^k−1​xk​=Axk−1​+Buk−1​+wk−1​
可知， e k − 1 e_{k-1} ek−1​作用于k-1时刻，而 w k − 1 w_{k-1} wk−1​作用于k时刻，因此两个概率是独立的，并且两者均满足高斯分布，因此它们的期望均为0，因此推导式可写为
P k − = E ( A e k − 1 e k − 1 T A T ) + E ( A e k − 1 ) E ( w k − 1 T ) + A T E ( w k − 1 ) E ( e k − 1 T ) + E ( w k − 1 w k − 1 T ) = E ( A e k − 1 e k − 1 T A T ) + 0 + 0 + E ( w k − 1 w k − 1 T ) = A E ( e k − 1 e k − 1 T ) A T + E ( w k − 1 w k − 1 T ) ⇒ = A P k − 1 A T + Q \begin{split} P_k^- &= E(Ae_{k-1}e_{k-1}^TA^T) + E(Ae_{k-1})E(w_{k-1}^T) + A^TE(w_{k-1})E(e_{k-1}^T) + E(w_{k-1}w_{k-1}^T)\\ &= E(Ae_{k-1}e_{k-1}^TA^T) + 0 + 0 + E(w_{k-1}w_{k-1}^T)\\ &= AE(e_{k-1}e_{k-1}^T)A^T + E(w_{k-1}w_{k-1}^T)\\ \Rightarrow&= AP_{k-1}A^T + Q\\ \end{split} Pk−​⇒​=E(Aek−1​ek−1T​AT)+E(Aek−1​)E(wk−1T​)+ATE(wk−1​)E(ek−1T​)+E(wk−1​wk−1T​)=E(Aek−1​ek−1T​AT)+0+0+E(wk−1​wk−1T​)=AE(ek−1​ek−1T​)AT+E(wk−1​wk−1T​)=APk−1​AT+Q​

卡尔曼滤波在控制领域应用的推导

控制领域的状态空间方程在实际应用过程中存在着两个问题，其一为表示系统状态的状态量在传输过程中存在着过程噪声 w w w，以及基于传感器等的测量的实际输出量存在着测量噪声 v v v。由于这两个未知量的引入，导致我们实际得到的数据与真实值存在一个波动的误差。带噪声的状态空间方程表示如下
{ x k = A x k − 1 + B u k − 1 + w k − 1 z k = H x k + v k \begin{cases} x_k = Ax_{k-1}+Bu_{k-1}+w_{k-1}\\ z_k = Hx_k + v_k \end{cases} {xk​=Axk−1​+Buk−1​+wk−1​zk​=Hxk​+vk​​
噪声的取值满足高斯分布，分别写作 P ( w ) ∼ N ( 0 , Q ) P(w)\sim N(0,Q) P(w)∼N(0,Q)以及 P ( w ) ∼ N ( 0 , R ) P(w)\sim N(0,R) P(w)∼N(0,R)。由于这两个噪声的存在，状态量与输出量就从原来的真实值变成了带偏差的估计值。根据原有的状态空间方程我们可以得到状态量的先验估计值(算出来的结果) x ^ k − \hat x_k^- x^k−​以及测量估计值（测出来的结果） x ^ k m e a s \hat x_{k_{meas}} x^kmeas​​，他们的计算公式如下
{ x k = A x k − 1 + B u k − 1 z k = H x k ⇒ { x ^ k − = A x k − 1 + B u k − 1 x ^ k m e a s = H − 1 z k \begin{cases} x_k = Ax_{k-1}+Bu_{k-1}\\ z_k = Hx_k \end{cases}\\ \Rightarrow \begin{cases} \hat x_k^- = Ax_{k-1}+Bu_{k-1}\\ \hat x_{k_{meas}} = H^{-1}z_k \end{cases} {xk​=Axk−1​+Buk−1​zk​=Hxk​​⇒{x^k−​=Axk−1​+Buk−1​x^kmeas​​=H−1zk​​
由于误差是无法通过建模得到的，因此只能引入概率论中的相关知识，由这两个带误差的结果得到一个更加接近真实值的结果（上节的推导）。根据之前的推导，它们之间存在如下关系
x ^ k = x ^ k − + G ( H − 1 z k − x ^ k − ) \hat x_k = \hat x_k^- + G(H^{-1}z_k - \hat x_k^-) x^k​=x^k−​+G(H−1zk​−x^k−​)
这种经过处理得到的更为接近的对真值的估计值被称作后验估计 x ^ k \hat x_k x^k​。为了方便理解，上式通常会进行该变换 G = K k H G=K_kH G=Kk​H，经该变换后 K k K_k Kk​的取值由原来的 [ 0 , 1 ] [0,1] [0,1]变为 [ 0 , H − 1 ] [0,H^{-1}] [0,H−1]。上式经整理后写作
x ^ k = x ^ k − + K k ( z k − H x ^ k − ) \hat x_k = \hat x_k^- + K_k(z_k - H\hat x_k^-) x^k​=x^k−​+Kk​(zk​−Hx^k−​)
此时我们的目标就变为了:寻找一个 K k K_k Kk​使得 x ^ k ⇒ x k \hat x_k \Rightarrow x_k x^k​⇒xk​，在概率论中表示即为 t r ( p ) tr(p) tr(p)最小。写作
e k = x k − x ^ k = x k − [ x ^ k − + K k ( z k − H x ^ k − ) ] = x k − x ^ k − − K k z k + K k H x ^ k − = x k − x ^ k − − K k ( H x k + v k ) + K k H x ^ k − = x k − x ^ k − − K k H x k − K k v k + K k H x ^ k − = ( x k − x ^ k − ) − K k H ( x k − x ^ k − ) − K k v k = ( I − K k H ) ( x k − x ^ k − ) − K k v k \begin{split} e_k &= x_k - \hat x_k\\ &= x_k -[\hat x_k^- + K_k(z_k - H\hat x_k^-)]\\ &= x_k -\hat x_k^- - K_kz_k + K_kH\hat x_k^-\\ &= x_k -\hat x_k^- - K_k(Hx_k + v_k) + K_kH\hat x_k^-\\ &= x_k -\hat x_k^- - K_kHx_k - K_kv_k + K_kH\hat x_k^-\\ &= (x_k -\hat x_k^-) - K_kH(x_k - \hat x_k^-) - K_kv_k \\ &= (I-K_kH)(x_k -\hat x_k^-) - K_kv_k \\ \end{split} ek​​=xk​−x^k​=xk​−[x^k−​+Kk​(zk​−Hx^k−​)]=xk​−x^k−​−Kk​zk​+Kk​Hx^k−​=xk​−x^k−​−Kk​(Hxk​+vk​)+Kk​Hx^k−​=xk​−x^k−​−Kk​Hxk​−Kk​vk​+Kk​Hx^k−​=(xk​−x^k−​)−Kk​H(xk​−x^k−​)−Kk​vk​=(I−Kk​H)(xk​−x^k−​)−Kk​vk​​
其中， x k − x ^ k − x_k -\hat x_k^- xk​−x^k−​可以写作误差的先验值 e k − e_k^- ek−​。假设误差 e k e_k ek​满足高斯分布，即 p ( e k ) ∼ N ( 0 ( 期望 ) , P ( 方差 ) ) p(e_k)\sim N(0(期望),P(方差)) p(ek​)∼N(0(期望),P(方差))，其方差可表示为
P k = E [ e k e k T ] = E [ ( x k − x ^ k ) ( x k − x ^ k ) T ] = E { [ ( I − K k H ) e k − − K k v k ] [ ( I − K k H ) e k − − K k v k ] T } = E { [ ( I − K k H ) e k − − K k v k ] [ e k − T ( I − K k H ) T − v k T K k T } = E { [ ( I − K k H ) e k − − K k v k ] [ e k − T ( I − K k H ) T − v k T K k T } = E [ ( I − K k H ) e k − e k − T ( I − K k H ) T − ( I − K k H ) e k − v k T K k T − v k K k e k − T ( I − K k H ) T + v k K k v k T K k T ] = E [ ( I − K k H ) e k − e k − T ( I − K k H ) T ] − E [ ( I − K k H ) e k − v k T K k T ] − E [ v k K k e k − T ( I − K k H ) T ] + E [ v k K k v k T K k T ] \begin{split} P_k &= E[e_ke_k^T]\\ &=E[(x_k -\hat x_k)(x_k -\hat x_k)^T]\\ &=E \lbrace [(I-K_kH)e_k^- - K_kv_k][(I-K_kH)e_k^- - K_kv_k]^T \rbrace\\ &=E \lbrace [(I-K_kH)e_k^- - K_kv_k] [e_k^{-T}(I-K_kH)^T - v_k^TK_k^T \rbrace\\ &=E \lbrace [(I-K_kH)e_k^- - K_kv_k] [e_k^{-T}(I-K_kH)^T - v_k^TK_k^T \rbrace\\ &=E [ (I-K_kH)e_k^-e_k^{-T}(I-K_kH)^T - (I-K_kH)e_k^-v_k^TK_k^T - v_kK_ke_k^{-T}(I-K_kH)^T + v_kK_kv_k^TK_k^T ]\\ &=E[(I-K_kH)e_k^-e_k^{-T}(I-K_kH)^T] - E[(I-K_kH)e_k^-v_k^TK_k^T] - E[v_kK_ke_k^{-T}(I-K_kH)^T] + E[v_kK_kv_k^TK_k^T]\\ \end{split} Pk​​=E[ek​ekT​]=E[(xk​−x^k​)(xk​−x^k​)T]=E{[(I−Kk​H)ek−​−Kk​vk​][(I−Kk​H)ek−​−Kk​vk​]T}=E{[(I−Kk​H)ek−​−Kk​vk​][ek−T​(I−Kk​H)T−vkT​KkT​}=E{[(I−Kk​H)ek−​−Kk​vk​][ek−T​(I−Kk​H)T−vkT​KkT​}=E[(I−Kk​H)ek−​ek−T​(I−Kk​H)T−(I−Kk​H)ek−​vkT​KkT​−vk​Kk​ek−T​(I−Kk​H)T+vk​Kk​vkT​KkT​]=E[(I−Kk​H)ek−​ek−T​(I−Kk​H)T]−E[(I−Kk​H)ek−​vkT​KkT​]−E[vk​Kk​ek−T​(I−Kk​H)T]+E[vk​Kk​vkT​KkT​]​
其中 ( I − K k H ) (I-K_kH) (I−Kk​H)和 K k T K_k^T KkT​都是常数，可以直接从期望计算中提取出来。第二项和第三项其实是等价的，进一步写为
2 ( I − K k H ) K k T ⋅ E ( e k − v k T ) 2(I-K_kH)K_k^T·E(e_k^-v_k^T) 2(I−Kk​H)KkT​⋅E(ek−​vkT​)
由于先验误差和测量误差是相互独立的，因此上式改写为
2 ( I − K k H ) K k T ⋅ E ( e k − ) E ( v k T ) 2(I-K_kH)K_k^T·E(e_k^-)E(v_k^T) 2(I−Kk​H)KkT​⋅E(ek−​)E(vkT​)
而二者都满足高斯分布，因此它们各自的期望值均为0，该项的结果为0。则原式子写为
P k = ( I − K k H ) E ( e k − e k − T ) ( I − K k H ) T − E [ ( I − K k H ) e k − v k T K k T ] + K k E ( v k v k T ) K k T = ( P k − − K k H P k − ) ( I − H T K k T ) + K k R K k T ⇒ = P k − − P k − H T K k T − K k H P k − + K k H P k − H T K k T + K k R K k T \begin{split} P_k &= (I-K_kH)E(e_k^-e_k^{-T})(I-K_kH)^T - E[(I-K_kH)e_k^-v_k^TK_k^T] + K_kE(v_kv_k^T)K_k^T\\ &= (P_k^- - K_kHP_k^-)(I - H^TK_k^T) + K_kRK_k^T\\ \Rightarrow&= P_k^- - P_k^-H^TK_k^T - K_kHP_k^- + K_kHP_k^-H^TK_k^T + K_kRK_k^T \end{split} Pk​⇒​=(I−Kk​H)E(ek−​ek−T​)(I−Kk​H)T−E[(I−Kk​H)ek−​vkT​KkT​]+Kk​E(vk​vkT​)KkT​=(Pk−​−Kk​HPk−​)(I−HTKkT​)+Kk​RKkT​=Pk−​−Pk−​HTKkT​−Kk​HPk−​+Kk​HPk−​HTKkT​+Kk​RKkT​​
t r ( A B ) tr(AB) tr(AB)为矩阵A与矩阵B相乘后对角线元素之和，改写为求解 P k P_k Pk​的迹的形式为
t r ( P k ) = t r ( P k − ) − 2 t r ( K k H P k − ) + t r ( K k H P k − H T K k T ) + t r ( K k R K k T ) tr(P_k) = tr(P_k^-) - 2tr(K_kHP_k^-) + tr(K_kHP_k^-H^TK_k^T) + tr(K_kRK_k^T) tr(Pk​)=tr(Pk−​)−2tr(Kk​HPk−​)+tr(Kk​HPk−​HTKkT​)+tr(Kk​RKkT​)
为了求解迹最小的情况，以下表述出其一阶导数形式
d t r ( P k ) d K k = 0 − 2 ( H P k − ) T + 2 K k H P k − H T + 2 K k R = P k − T H T + K k ( H P k − H T + R ) （协方差矩阵的转置等于它自己） = P k − T H + K k ( H P k − H T + R ) \begin{split} \frac{dtr(P_k)}{dK_k} &= 0 - 2(HP_k^-)^T + 2K_kHP_k^-H^T + 2K_kR\\ &= P_k^{-T}H^T + K_k(HP_k^-H^T + R)\\ （协方差矩阵的转置等于它自己）&= P_k^{-T}H + K_k(HP_k^-H^T + R)\\ \end{split} dKk​dtr(Pk​)​（协方差矩阵的转置等于它自己）​=0−2(HPk−​)T+2Kk​HPk−​HT+2Kk​R=Pk−T​HT+Kk​(HPk−​HT+R)=Pk−T​H+Kk​(HPk−​HT+R)​
上式求解中涉及到一个矩阵求偏导的变换，写作
d t r ( A B ) d A = B T \frac{dtr(AB)}{dA} = B^T dAdtr(AB)​=BT
其证明如下
t r ( A B ) = A ⋅ B = [ a 11 a 12 a 21 a 22 ] [ b 11 b 12 b 21 b 22 ] 取对角元素之和 = a 11 b 11 + a 12 b 21 + a 21 b 12 + a 22 b 22 d t r ( A B ) d A = [ ∂ t r ( A B ) ∂ a 11 ∂ t r ( A B ) ∂ a 12 ∂ t r ( A B ) ∂ a 21 ∂ t r ( A B ) ∂ a 22 ] = [ b 11 b 12 b 21 b 22 ] = B T \begin{split} tr(AB) &= A·B \\ &= \begin{bmatrix} a_{11}&a_{12}\\a_{21}&a_{22} \end{bmatrix} \begin{bmatrix} b_{11}&b_{12}\\b_{21}&b_{22} \end{bmatrix}\\ 取对角元素之和&= a_{11}b_{11} + a_{12}b_{21} + a_{21}b_{12} + a_{22}b_{22}\\ \frac{dtr(AB)}{dA} &= \begin{bmatrix} \frac{\partial tr(AB)}{\partial a_{11}}& \frac{\partial tr(AB)}{\partial a_{12}}\\ \frac{\partial tr(AB)}{\partial a_{21}}& \frac{\partial tr(AB)}{\partial a_{22}}\\ \end{bmatrix}\\ &= \begin{bmatrix} b_{11}&b_{12}\\ b_{21}&b_{22}\\ \end{bmatrix}\\ &= B^T \end{split} tr(AB)取对角元素之和dAdtr(AB)​​=A⋅B=[a11​a21​​a12​a22​​][b11​b21​​b12​b22​​]=a11​b11​+a12​b21​+a21​b12​+a22​b22​=[∂a11​∂tr(AB)​∂a21​∂tr(AB)​​∂a12​∂tr(AB)​∂a22​∂tr(AB)​​]=[b11​b21​​b12​b22​​]=BT​
当一阶导为零时，此时求得的值为迹的最小（最优解），整理如下
d t r ( P k ) d K k = P k − T H + K k ( H P k − H T + R ) = 0 ⇒ 0 − 2 ( h P k − ) T + 2 K k H P k − H T + 2 K k R = 0 − P k − H T + K k ( H P k − + R ) = 0 K k ( H P k − H T + R ) = P k − H T ⇒ K k = P k − H T H P k − H T + R \frac{dtr(P_k)}{dK_k} = P_k^{-T}H + K_k(HP_k^-H^T + R) = 0\\ \begin{split} \Rightarrow 0 - 2(hP_k^-)^T + 2K_kHP_k^-H^T + 2K_kR &= 0\\ -P_k^-H^T + K_k(HP_k^-+R) &= 0\\ K_k(HP_k^-H^T + R) &= P_k^-H^T\\ \Rightarrow K_k = \frac{P_k^-H^T}{HP_k^-H^T + R} \end{split} dKk​dtr(Pk​)​=Pk−T​H+Kk​(HPk−​HT+R)=0⇒0−2(hPk−​)T+2Kk​HPk−​HT+2Kk​R−Pk−​HT+Kk​(HPk−​+R)Kk​(HPk−​HT+R)⇒Kk​=HPk−​HT+RPk−​HT​​=0=0=Pk−​HT​

卡尔曼增益详解

卡尔曼增益的计算方式

估计误差 ( e s t i m a t e ) e E S T = ∣ x − x ^ ∣ 测量误差 ( m e a s u r e ) e M E A = ∣ x − z ∣ 卡尔曼增益 K k = e E S T k − 1 e E S T k − 1 + e M E A k 估计误差(estimate)\hspace{0.5cm}e_{EST} = |x - \hat x|\\ 测量误差(measure)\hspace{0.5cm}e_{MEA} = |x - z|\\ 卡尔曼增益\hspace{0.5cm}K_k = \frac{e_{EST_{k-1}}}{e_{EST_{k-1}}+e_{MEA_k} } 估计误差(estimate)eEST​=∣x−x^∣测量误差(measure)eMEA​=∣x−z∣卡尔曼增益Kk​=eESTk−1​​+eMEAk​​eESTk−1​​​

卡尔曼增益的解读

在k时刻，存在两种情况
① k-1时刻的估计误差远远大于k时的测量误差，即
e E S T k − 1 ≫ e M E A k e_{EST_{k-1}} \gg e_{MEA_k} eESTk−1​​≫eMEAk​​
有以下关系
{ K k ⇒ 1 x ^ k = x ^ k − 1 + z k − x ^ k − 1 = z k \begin{cases} \begin{split} K_k&\Rightarrow1\\ \hat x_k &= \hat x_{k-1} + z_k - \hat x_{k-1} \\ &= z_k \end{split} \end{cases} ⎩ ⎨ ⎧​Kk​x^k​​⇒1=x^k−1​+zk​−x^k−1​=zk​​​
此时第k次的估计真值等于第k次的测量值
② k-1时刻的估计误差远远小于k时的测量误差
e E S T k − 1 ≪ e M E A k e_{EST_{k-1}} \ll e_{MEA_k} eESTk−1​​≪eMEAk​​
有以下关系
{ K k ⇒ 0 x ^ k = x ^ k − 1 \begin{cases} K_k\Rightarrow0\\ \hat x_k = \hat x_{k-1} \end{cases} {Kk​⇒0x^k​=x^k−1​​
此时第k次的估计真值等于上一次的估计真值。

卡尔曼滤波的应用

普通应用中的一般流程

  1. 计算卡尔曼增益 K k K_k Kk​
    K k = e E S T k − 1 e E S T k − 1 + e M E A k K_k = \frac{e_{EST_{k-1}}}{e_{EST_{k-1}}+e_{MEA_k}} Kk​=eESTk−1​​+eMEAk​​eESTk−1​​​
  2. 计算当前的估计值 x ^ k \hat x_k x^k​
    x ^ k = x ^ k − 1 + K k ⋅ ( z k − x ^ k − 1 ) \hat x_k= \hat x_{k-1}+K_k·(z_k - \hat x_{k-1}) x^k​=x^k−1​+Kk​⋅(zk​−x^k−1​)
  3. 更新当前的估计误差 e E S T k e_{EST_k} eESTk​​
    e E S T k = ( 1 − K k ) ⋅ e E S T k − 1 e_{EST_k} = (1-K_k)·e_{EST_{k-1}} eESTk​​=(1−Kk​)⋅eESTk−1​​

实例说明

生产硬币的机器在实际生产时会由于各种影响因素导致误差生产出来的硬币直径与设计尺寸存在一定误差（误差1），当我们对真实硬币的直径进行测量时，每次也会引入一定误差（误差2）。利用卡尔曼滤波对硬币真实的直径进行预测。

应用实例（matlab）：预测实际硬币的直径

clear;clc;close all;
%% 测量硬币的直径，已知条件
x = 50;          % 实际长度,mm
hat_x = 45;    % 第一次估计值
est_error = 5;  % 估计误差
z = [51 48 47 52 51 48 49 53 48 49 52 53 51 52 49 50];      % 测量长度
mea_error = 3;   % 测量误差，即测量值在真实值±3mm内波动
%% main
k = 16;
K_k = zeros(k,1);
for i = 1:k
    K_k(i) = est_error/(est_error + mea_error); % Kalman_Gain
    hat_x = hat_x + K_k(i)*(z(i)-hat_x);
    Hat_x(i) = hat_x;
    est_error = (1-K_k(i))*est_error;
    Est_error(i) = est_error;
end
figure;
plot(ones(1,17)*x,'g-');hold on;
plot(z,'ro-');hold on;
plot([45,Hat_x],'bo-')
legend("真实值","测量值","滤波后的估计值",'Location','southeast')

在这里插入图片描述

悠以悠
关注
  • 2
    点赞
  • 踩
  • 4
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
专栏目录
卡尔曼滤波实例
07-19
学习卡尔曼滤波必备
卡尔曼滤波系列_实例（二）均加速运动的卡尔曼滤波
a1240563221的博客
03-05 2071
对均加速运动的卡尔曼滤波进行介绍
kalman filter 卡尔曼滤波的例子
06-28 5365
因为在研究中使用了kalman 滤波，这是一个挺难理解的控制理论，我花了好长时间才了解一些基本的概念，opencv虽然提供了一个实例，但是这个例子基于c的，不容易看懂，也不好重用，后来我整理了一个简单的类来，期间在论坛上有一个handsome & romantic 的法国小伙也在研究这个滤波，后来我给了他程序，他修改后发给了我，所以这里的代码也有他的部分，算是中法合作产品 :)对于kalman的初
卡尔曼滤波器KF
奉现的博客
08-01 243
卡尔曼滤波器及其推导
卡尔曼滤波Kalman Filter
01-06
年少不知记笔记，老来方恨看不懂……本文从三方面进行，首先是卡尔曼滤波的思想，然后对卡尔曼滤波进行公式推导，最后是对卡尔曼滤波进行总结。 核心思想 取自知乎Kent Zeng的回答：点击链接，我对他的回答进行了...
Kalman Filter卡尔曼滤波 java实现
08-09
KalmanFilter,KalmanFilter3,KalmanFilter5 一共7种,以上三种实现保证可用,其余待调试,调试函数写好,直接运行即可 KalmanFilter KalmanFilter2 KalmanFilter3 KalmanFilter4 KalmanFilter5 KalmanFilter6 Kalman...
卡尔曼滤波_Kalmanfilter_amsyk_卡尔曼滤波_verilog_卡尔曼verilog
09-10
标题中的"卡尔曼滤波_Kalmanfilter_amsyk_卡尔曼滤波_verilog_卡尔曼verilog"暗示了这个项目是关于使用Verilog硬件描述语言实现卡尔曼滤波器。Verilog是一种广泛用于数字电路设计的语言，它可以用来描述和模拟数字...
卡尔曼滤波（Kalman Filter）原理与公式推导.pdf
12-23
卡尔曼滤波（Kalman Filter）原理与公式推导.pdf
卡尔曼滤波的实例集合
08-10
卡尔曼滤波的实例集合，适用于各种预测、数值估计、拟合等问题的解决，有详细的代码辅助说明。
卡尔曼滤波的小例子 很实用 初学者 MATLAB m文件编辑
09-19
卡尔曼滤波的小例子 很实用 初学者 MATLAB m文件编辑
卡尔曼滤波器matlab实例
10-26
卡尔曼滤波器matlab实例 有助于初学者了解卡尔曼滤波器的计算流程
基于蒙特卡罗的卡尔曼滤波实例
11-04
卡尔曼滤波的一个小范例，采用蒙特卡洛仿真实现轨迹平滑，对于初学卡尔曼滤波信息融合技术的同学有很大帮助
一个应用实例详解卡尔曼滤波及其算法
12-05
一个应用实例详解卡尔曼滤波及其算法实现,为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。
卡尔曼滤波 kalman filter
07-04
滤波；卡尔曼滤波；kalman filter
卡尔曼滤波示例
honyniu的专栏
03-20 8366
在上一篇卡尔曼滤波详解已经给出详细的推导过程。这里给出几个示例加深理解，这个是几年前学习卡尔曼滤波时候整理的，比较啰嗦，初学者可以看看，了解过的就不用浪费时间了，主要是最近在整理资料差点弄丢，还是放到网上保存吧。 简单示例1 这个示例中假设状态是稳定的，也就是真实的状态是不随着时间变化的，那么利用卡尔曼滤波算法最后会收敛到最优的状态。 参数设置 对于所有的变量，本场景下都是一个具体的数值而不是矩阵...
【卡尔曼滤波】卡尔曼滤波简单实例
一些笔记
10-24 3758
卡尔曼滤波的一个简单示例，有助于对其的理解。
卡尔曼滤波的使用举例
VictorHan01的博客
07-06 8774
卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。 顺带提一下协方差矩阵，下面会用到。 协方差矩阵： 以下来源：https://blog.csdn.net/su_bao/article/details/80997077 ...
卡尔曼滤波(kalman filter)原理与公式推导卡尔曼滤波算法详细推导
最新发布
09-04
卡尔曼滤波是一种用来估计系统状态的递归滤波算法，适用于线性系统且满足高斯分布的噪声。该滤波器是由R. E. Kalman提出的。 卡尔曼滤波的原理是基于两个假设：系统动态方程能由线性方程描述，测量方程能由线性方程描述。在每个时间步，卡尔曼滤波器通过两个步骤进行估计和更新：预测步骤和校正步骤。预测步骤是根据系统动态方程和上一个时间步的估计状态预测当前状态的均值和方差。校正步骤是根据测量方程和当前观测得到的测量值以及预测的状态，利用贝叶斯定理更新状态的均值和方差，得到最终的估计值。 推导卡尔曼滤波算法的公式如下： 预测步骤： 预测状态： $ x^- = A \cdot x + B \cdot u $ 预测状态协方差矩阵： $ P^- = A \cdot P \cdot A^T + Q $ 校正步骤： 卡尔曼增益： $ K = P^- \cdot H^T \cdot (H \cdot P^- \cdot H^T + R)^{-1} $ 修正后的状态： $ x = x^- + K \cdot (z - H \cdot x^-) $ 修正后的状态协方差矩阵： $ P = (I - K \cdot H) \cdot P^- $ 其中，x是系统状态向量，A是状态转移矩阵，B是输入矩阵，u是输入向量，P是后验状态的误差协方差矩阵，Q是预测误差协方差矩阵，H是测量矩阵，R是测量误差的协方差矩阵，z是观测向量。 通过上述公式的迭代，卡尔曼滤波器可以递归地估计系统的状态，并通过校正步骤利用最新的观测值来更新估计值。这种算法在估计方差较大的实时系统中具有优势，可以去除噪声和不确定性，提高系统的估计精度。
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值