Kalman滤波
文章目录
- 一、公式推导
- 二、扩展卡尔曼滤波
卡尔曼滤波是一种最优化递归数据处理算法。（Optimal Recursive Data Processing Algorithm）
Kalman滤波是时域滤波，采用状态空间描述系统，运用递推形式是计算简单，数据存储量小，应用广泛。
广泛应用于惯性导航、制导系统、全球定位系统、目标跟踪、通信与信号处理、金融等。
Kalman滤波器的广泛应用是因为我们的生活中存在大量不确定性。
在我们描述一个系统时，不确定性主要体现在3个方面：
- 不存在完美的数学模型
- 系统的扰动不可控，也很难建模
- 测量传感器本身存在误差
一、公式推导
状态空间方程：
x k = A x k − 1 + B u k − 1 + w k − 1 (1) x_k=Ax_{k-1}+Bu_{k-1}+w_{k-1} \tag{1} xk=Axk−1+Buk−1+wk−1(1)
z k = H x k + v k (2) z_k=Hx_k+v_k \tag{2} zk=Hxk+vk(2)
- w k − 1 w_{k-1} wk−1为过程噪声，不可测，但我们可以假设其符合正态分布 P ( w ) ∼ ( 0 , Q ) P(w)\sim(0,Q) P(w)∼(0,Q)，0为期望，Q为协方差矩阵。 Q = E [ w w T ] Q=E[ww^T] Q=E[wwT]
- v k v_k vk为测量噪声。 P ( v ) ∼ ( 0 , R ) P(v)\sim(0,R) P(v)∼(0,R), R = E [ v v T ] R=E[vv^T] R=E[vvT]
- 在实际建模过程中， w k − 1 w_{k-1} wk−1和 v k v_k vk项是无法建模的，只知道前面的项，所以只能有估计值。
x ^ k − = A x k − 1 + B u k − 1 (3) \hat x_k^-=Ax_{k-1}+Bu_{k-1}\tag{3} x^k−=Axk−1+Buk−1(3)
- x ^ k − \hat x_k^- x^k−为先验估计，通过状态空间方程去掉过程噪声得到的式子，是计算出来的。
由 z k = H x k z_k=Hx_k zk=Hxk可得 x ^ k M E A = H − 1 z k (4) \hat x_{k_{MEA}}=H^{-1}z_k\tag{4} x^kMEA=H−1zk(4)
- 测量结果 z k z_k zk已知， x ^ k M E A \hat x_{k_{MEA}} x^kMEA是测出来的。
无论是算出来的 x ^ k − \hat x_k^- x^k−还是测出来的 x ^ k M E A \hat x_{k_{MEA}} x^kMEA，都不具备噪声项，利用数据融合可得
x ^ k = x ^ k − + G ( H − 1 z k − x k − ) , G = K k H \hat x_k=\hat x_k^-+G(H^{-1}z_k-x_k^-),G=K_kH x^k=x^k−+G(H−1zk−xk−),G=KkH
- G = 0 G=0 G=0时， x ^ k = x ^ k − \hat x_k=\hat x_k^- x^k=x^k−
- G = 1 G=1 G=1时， x ^ k = H − 1 z k \hat x_k=H^{-1}z_k x^k=H−1zk
x ^ k = x ^ k − + K k ( z k − H x k − ) (5) \hat x_k=\hat x_k^-+K_k(z_k-Hx_k^-)\tag{5} x^k=x^k−+Kk(zk−Hxk−)(5)
- K k = 0 K_k=0 Kk=0时， x ^ k = x ^ k − \hat x_k=\hat x_k^- x^k=x^k−
- K k = H − K_k=H^- Kk=H−时， x ^ k = H − 1 z k \hat x_k=H^{-1}z_k x^k=H−1zk
目标：寻找 K k K_k Kk使得 x ^ k → x k \hat x_k\to x_k x^k→xk, x k x_k xk为实际值。
引入 e k = x k − x ^ k (6) e_k=x_k-\hat x_k\tag{6} ek=xk−x^k(6)
-
P ( e k ) ∼ ( 0 , P ) P(e_k)\sim(0,P) P(ek)∼(0,P)
P = E [ e e T ] = [ σ e 1 2 σ e 1 σ e 2 σ e 2 σ e 1 σ e 2 2 ] (7) P=E[ee^T]=\begin{bmatrix}\sigma e_1^2 & \sigma e_1\sigma e_2 \\ \sigma e_2\sigma e_1 & \sigma e_2^2 \end{bmatrix} \tag{7} P=E[eeT]=[σe12σe2σe1σe1σe2σe22](7) -
t r ( P ) = σ e 1 2 + σ e 2 2 tr(P)=\sigma e_1^2+\sigma e_2^2 tr(P)=σe12+σe22，目标即为使得 t r ( P ) tr(P) tr(P)最小
x k − x ^ k = 【代入 ( 5 ) 】 x k − ( x ^ k − + K k ( z k − H x k − ) ) = x k − x k − − K k z k + K k H x k − = 【代入 ( 2 ) 】 x k − x k − − K k ( H x k + v k ) + K k H x k − = ( I − K k H ) ( x k − x k − ) − K k v k = ( I − K k H ) e k − − K k v k \begin{aligned} \color{green}x_k-\hat x_k&=【代入(5)】x_k-(\hat x_k^-+K_k(z_k-Hx_k^-)) \\&=x_k-x_k^--K_kz_k+K_kHx_k^- \\&=【代入(2)】x_k-x_k^--K_k(Hx_k+v_k)+K_kHx_k^- \\&=(I-K_kH)(x_k-x_k^-)-K_kv_k \\&= \color{green}(I-K_kH)e_k^--K_kv_k \end{aligned} xk−x^k=【代入(5)】xk−(x^k−+Kk(zk−Hxk−))=xk−xk−−Kkzk+KkHxk−=【代入(2)】xk−xk−−Kk(Hxk+vk)+KkHxk−=(I−KkH)(xk−xk−)−Kkvk=(I−KkH)ek−−Kkvk
E [ ( I − K k H ) e k − v k T K k T ] = ( I − K k H ) E ( e k − v k T ) K k T = ( I − K k H ) E ( e k − ) E ( v k T ) K k T 【 E ( e k − ) = 0 ， E ( v k T ) = 0 】 = 0 \begin{aligned}\color{blue}E[(I-K_kH)e_k^-v_k^TK_k^T]&=(I-K_kH)E(e_k^-v_k^T)K_k^T \\&=(I-K_kH)E(e_k^-)E(v_k^T)K_k^T \ \ \ 【E(e_k^-)=0，E(v_k^T)=0】 \\&=\color{blue}0 \end{aligned} E[(I−KkH)ek−vkTKkT]=(I−KkH)E(ek−vkT)KkT=(I−KkH)E(ek−)E(vkT)KkT 【E(ek−)=0，E(vkT)=0】=0
E [ K k v k e k − T ( I − K k H ) T ] = 0 【理由同上】 \color{blue}E[K_kv_ke_k^{-T}(I-K_kH)^T]=\color{blue}0【理由同上】 E[Kkvkek−T(I−KkH)T]=0【理由同上】
P k = E [ e e 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 − e k − T ( I − K k H ) T − ( I − K k H ) e k − v k T K k T − K k v k e k − T ( I − K k H ) T + K k v 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 [ K k v k e k − T ( I − K k H ) T ] + E [ K k v k v k T K k T ] = ( I − K k H ) E ( e k − e k − T ) ( I − K k H ) T + K k E ( v k v k T ) K k T = 【 E ( e k − e k − T ) = P k − ， E ( v k v k T ) = R 】（ P k − − K k H P k − ） ( I − K k H ) T + K k R K k T = P k − − K k H P k − − P k − H T K k T + K k H P k − H T K k T + K k R K k T \begin{aligned} P_k & =E[ee^T] \\ &=E[({\color{green}x_k-\hat x_k})({\color{green}x_k-\hat x_k})^T] \\ &=E[[(I-K_kH)e_k^--K_kv_k][(I-K_kH)e_k^--K_kv_k]^T] \\ &=E[[(I-K_kH)e_k^--K_kv_k][e_k^{-T}(I-K_kH)^T-v_k^TK_k^T]] \\ &=E[(I-K_kH)e_k^-e_k^{-T}(I-K_kH)^T-(I-K_kH)e_k^-v_k^TK_k^T-K_kv_ke_k^{-T}(I-K_kH)^T+K_kv_kv_k^TK_k^T] \\ &=E[(I-K_kH)e_k^-e_k^{-T}(I-K_kH)^T]-{\color{blue}E[(I-K_kH)e_k^-v_k^TK_k^T]}-{\color{blue}E[K_kv_ke_k^{-T}(I-K_kH)^T]}+E[K_kv_kv_k^TK_k^T] \\ &=(I-K_kH)E(e_k^-e_k^{-T})(I-K_kH)^T+K_kE(v_kv_k^T)K_k^T \\ &=【E(e_k^-e_k^{-T})=P_k^-，E(v_kv_k^T)=R】（P_k^--K_kHP_k^-）(I-K_kH)^T+K_kRK_k^T \\ &=P_k^--{\color{purple}K_kHP_k^-}-{\color{red}P_k^-H^TK_k^T}+K_kHP_k^-H^TK_k^T+K_kRK_k^T \end{aligned} Pk=E[eeT]=E[(xk−x^k)(xk−x^k)T]=E[[(I−KkH)ek−−Kkvk][(I−KkH)ek−−Kkvk]T]=E[[(I−KkH)ek−−Kkvk][ek−T(I−KkH)T−vkTKkT]]=E[(I−KkH)ek−ek−T(I−KkH)T−(I−KkH)ek−vkTKkT−Kkvkek−T(I−KkH)T+KkvkvkTKkT]=E[(I−KkH)ek−ek−T(I−KkH)T]−E[(I−KkH)ek−vkTKkT]−E[Kkvkek−T(I−KkH)T]+E[KkvkvkTKkT]=(I−KkH)E(ek−ek−T)(I−KkH)T+KkE(vkvkT)KkT=【E(ek−ek−T)=Pk−，E(vkvkT)=R】（Pk−−KkHPk−）(I−KkH)T+KkRKkT=Pk−−KkHPk−−Pk−HTKkT+KkHPk−HTKkT+KkRKkT
( P k − H T K k T ) T = K k ( P k − H T ) T = K k H P k − 【故这两项的迹相等】 \begin{aligned}({\color{red}P_k^-H^TK_k^T})^T&=K_k(P_k^-H^T)^T \\&={\color{purple}K_kHP_k^-} 【故这两项的迹相等】 \end{aligned} (Pk−HTKkT)T=Kk(Pk−HT)T=KkHPk−【故这两项的迹相等】
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(KkHPk−)+tr(KkHPk−HTKkT)+tr(KkRKkT)
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 \frac{dtr(P_k)}{dK_k}=0-2(HP_k^-)^T+2K_kHP_k^-H^T+2K_kR dKkdtr(Pk)=0−2(HPk−)T+2KkHPk−HT+2KkR
令 d t r ( P k ) d K k = 0 \frac{dtr(P_k)}{dK_k}=0 dKkdtr(Pk)=0得
− 2 ( H P k − ) T + 2 K k H P k − H T + 2 K k R = 0 -2(HP_k^-)^T+2K_kHP_k^-H^T+2K_kR=0 −2(HPk−)T+2KkHPk−HT+2KkR=0
− P k − T H T + K k H P k − H T + K k R = 0 -P_k^{-T}H^T+K_kHP_k^-H^T+K_kR=0 −Pk−THT+KkHPk−HT+KkR=0
【协方差矩阵的转置等于其本身】 【协方差矩阵的转置等于其本身】 【协方差矩阵的转置等于其本身】
− P k − H T + K k H P k − H T + K k R = 0 -P_k^-H^T+K_kHP_k^-H^T+K_kR=0 −Pk−HT+KkHPk−HT+KkR=0
K k ( H P k − H T + R ) = P k − H T K_k(HP_k^-H^T+R)=P_k^-H^T Kk(HPk−HT+R)=Pk−HT
K k = P k − H T H P k − H T + R K_k=\frac{P_k^-H^T}{HP_k^-H^T+R} Kk=HPk−HT+RPk−HT
- R较大时， K k → 0 , x ^ k = x ^ k − K_k \to 0,\hat x_k=\hat x_k^- Kk→0,x^k=x^k−
- R较小时， K k = H − ， x ^ k = H − 1 z k K_k=H^-，\hat x_k=H^{-1}z_k Kk=H−，x^k=H−1zk
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 − 1 ) + w k − 1 = A e k − 1 + w k − 1 \begin{aligned}{\color{brown}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-1})+w_{k-1} \\ &=\color{brown}Ae_{k-1}+w_{k-1} \end{aligned} ek−=xk−x^k−=Axk−1+Buk−1+wk−1−Ax^k−1−Buk−1=A(xk−1−x^k−1)+wk−1=Aek−1+wk−1
E [ A e k − 1 w k − 1 T ] = 【相互独立】 A E [ e k − 1 ] E [ w k − 1 T ] = 【 E [ e k − 1 ] = 0 ， E [ w k − 1 T = 0 】 A ⋅ 0 ⋅ 0 = 0 \begin{aligned}{\color{fuchsia}E[Ae_{k-1}w_{k-1}^T]}&=【相互独立】AE[e_{k-1}]E[w_{k-1}^T] \\&=【E[e_{k-1}]=0，E[w_{k-1}^T=0】A\cdot0\cdot0 \\&=\color{fuchsia}0 \end{aligned} E[Aek−1wk−1T]=【相互独立】AE[ek−1]E[wk−1T]=【E[ek−1]=0，E[wk−1T=0】A⋅0⋅0=0
E [ w k − 1 e k − 1 T A T ] = 0 【理由同上】 \color{fuchsia}E[w_{k-1}e_{k-1}^TA^T]=0【理由同上】 E[wk−1ek−1TAT]=0【理由同上】
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 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 ] = E [ A e k − 1 e k − 1 T A T ] + 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{aligned}P_k^- &=E[{\color{brown}e_k^-}e_k^{-T}] \\&=E[(Ae_{k-1}+w_{k-1})(Ae_{k-1}+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]+{\color{fuchsia}E[Ae_{k-1}w_{k-1}^T]}+{\color{fuchsia}E[w_{k-1}e_{k-1}^TA^T]}+E[w_{k-1}w_{k-1})^T] \\&=E[Ae_{k-1}e_{k-1}^TA^T]+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] \\&=AP_{k-1}A^T+Q \end{aligned} Pk−=E[ek−ek−T]=E[(Aek−1+wk−1)(Aek−1+wk−1)T]=E[Aek−1ek−1TAT+Aek−1wk−1T+wk−1ek−1TAT+wk−1wk−1)T]=E[Aek−1ek−1TAT]+E[Aek−1wk−1T]+E[wk−1ek−1TAT]+E[wk−1wk−1)T]=E[Aek−1ek−1TAT]+E[wk−1wk−1)T]=AE[ek−1ek−1T]AT+E[wk−1wk−1)T]=APk−1AT+Q
利用卡尔曼滤波器估计状态变量的值
二、扩展卡尔曼滤波
对于非线性系统：
x k = f ( x k − 1 , u k − 1 ， w k − 1 ) x_k=f(x_{k-1},u_{k-1}，w_{k-1}) xk=f(xk−1,uk−1，wk−1)
z k = h ( x k , v k ) z_k=h(x_k,v_k) zk=h(xk,vk)
由于正态分布的随机变量通过非线性系统后就不再是正态的了，所以如果想使用Kalman滤波，就需要对其线性化。使用Tylor Series(泰勒级数)展开。 f ( x ) = f ( x 0 ) + ∂ f ∂ x ( x − x 0 ) f(x)=f(x_0)+\frac{\partial f}{\partial x}(x-x_0) f(x)=f(x0)+∂x∂f(x−x0)
系统有误差，无法在真实点线性化。
f ( x k ) f(x_k) f(xk)在 x ^ k − 1 \hat x_{k-1} x^k−1( k − 1 k-1 k−1时的后验估计)处线性化
x k = f ( x ^ k − 1 , u k − 1 , w k − 1 ) + A ( x k − x ^ k − 1 ) + w k w k − 1 x_k={\color{red}f(\hat x_{k-1},u_{k-1},w_{k-1})}+{\color{green}A(x_k-\hat x_{k-1})}+{\color{blue}w_kw_{k-1}} xk=f(x^k−1,uk−1,wk−1)+A(xk−x^k−1)+wkwk−1
- f ( x ^ k − 1 , u k − 1 , w k − 1 ) = f ( x ^ k − 1 , u k − 1 , 0 ) = x ~ k \color{red}f(\hat x_{k-1},u_{k-1},w_{k-1})=f(\hat x_{k-1},u_{k-1},0)=\tilde x_k f(x^k−1,uk−1,wk−1)=f(x^k−1,uk−1,0)=x~k
- A 为雅可比矩阵， A = ∂ f ∂ x ∣ x ^ k − 1 , u k − 1 \color{green}A为雅可比矩阵，A=\frac{\partial f}{\partial x}_{|\hat x_{k-1},u_{k-1}} A为雅可比矩阵，A=∂x∂f∣x^k−1,uk−1
- w k = ∂ f ∂ w ∣ x ^ k − 1 , u k − 1 \color{blue}w_k=\frac{\partial f}{\partial w}_{|\hat x_{k-1},u_{k-1}} wk=∂w∂f∣x^k−1,uk−1
z k z_k zk在 x ~ k \tilde x_k x~k线性化
z k = h ( x ~ k , v k ) + H ( x k − x ~ k ) + V v k z_k=h(\tilde x_k,v_k)+H(x_k-\tilde x_k)+Vv_k zk=h(x~k,vk)+H(xk−x~k)+Vvk
参考：
卡尔曼滤波DR_CAN