卡尔曼滤波学习记录

最新推荐文章于 2024-07-26 13:24:37 发布
最新推荐文章于 2024-07-26 13:24:37 发布
阅读量279 收藏 2
点赞数
分类专栏： 无人驾驶 文章标签： 无人驾驶 卡尔曼滤波 人工智能
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/cmf8797143/article/details/88063576
版权

卡尔曼滤波学习记录

前言

博主是做无人驾驶车辆建图与定位的，所以针对于车辆方面来浅显的学习。
这一部分是关于卡尔曼滤波器的理论部分，言简意赅，只写关键的东西。
卡尔曼滤波的核心就是预测与更新。预测一个理论值，再根据测量值去修正这个理论值。
预测的值称为预测值，修正后的值称为估计值。

推理过程

方程

卡尔曼滤波主要用于测量数据与推测数据的融合，浅显的解释就是，我用一个系统算了一个值，比如说车速，我还有一个传感器能够测一个车速，这两个速度值都存在着不同程度的误差，那么我就把他俩融合起来，求一个比较准确的速度。

处理方程

处理方程就是抽象实际情况中的系统状态，得到处理模型方程。
一般我们遇到的处理方程是这样：
x k = A x k − 1 + B u k + ϵ \LARGE x_k=Ax_{k-1}+Bu_k+\epsilon xk​=Axk−1​+Buk​+ϵ
其中传递的意思就是，该系统 k k k时刻的状态 x k x_k xk​由 k − 1 k-1 k−1时刻的状态 x k − 1 x_{k-1} xk−1​与转换矩阵 A A A作用 + + +输入的数据 u k u_k uk​与转换矩阵 B B B作用+噪声 ϵ \epsilon ϵ。
注意：该方程也常称为预测方程，即我们通过这个方程算出来的 x t x_t xt​是一个理论值。真实情况还需要用理论结合实际测量值计算出来。

用一个匀加速直线运动举例，根据物理知识，易得方程组
S k = S k − 1 + V k − 1 Δ t + 1 2 a ( Δ t ) 2 \LARGE S_k=S_{k-1}+V_{k-1}\Delta t+\frac{1}{2}a(\Delta t)^2 Sk​=Sk−1​+Vk−1​Δt+21​a(Δt)2
V k = V k − 1 + a Δ t \LARGE V_k=V_{k-1}+a\Delta t Vk​=Vk−1​+aΔt
S S S是位移， V V V是速度， a a a是加速度， Δ t \Delta t Δt是间隔时间，即 k k k时刻与 k − 1 k-1 k−1时刻的时间差值
把这个方程组和状态方程进行类比，我们可以得到
[ S k V k ] = [ 1 Δ t 0 1 ] ∗ [ S k − 1 V k − 1 ] + [ 1 2 a ( Δ t ) 2 Δ t ] ∗ a \LARGE \begin{bmatrix} S_{k}\\V_{k} \end{bmatrix}\quad =\begin{bmatrix}1&\Delta t\\0&1\end{bmatrix}*\begin{bmatrix}S_{k-1}\\V_{k-1}\end{bmatrix} +\begin{bmatrix}\frac{1}{2}a(\Delta t)^2\\\Delta t\end{bmatrix}*a ⎣⎢⎡​Sk​Vk​​⎦⎥⎤​=⎣⎢⎡​10​Δt1​⎦⎥⎤​∗⎣⎢⎡​Sk−1​Vk−1​​⎦⎥⎤​+⎣⎢⎡​21​a(Δt)2Δt​⎦⎥⎤​∗a
状态方程里面的 x k = [ S k V k ] x_k=\begin{bmatrix} S_{k}\\V_{k} \end{bmatrix} xk​=[Sk​Vk​​] A = [ 1 Δ t 0 1 ] A=\begin{bmatrix}1&\Delta t \\0&1\end{bmatrix} A=[10​Δt1​] B = [ 1 2 ( Δ t ) 2 Δ t ] B=\begin{bmatrix}\frac{1}{2}(\Delta t)^2\\\Delta t\end{bmatrix} B=[21​(Δt)2Δt​] u k = a u_k=a uk​=a
此时我们并没有考虑噪声的存在情况，及处理方程里面的 ϵ t \epsilon_t ϵt​
既然我们现在有了处理方程，那么直接进行计算不就行了吗？为啥还要参考测量值呢？那是因为真实的世界环境中，一个系统的状态非常复杂，往往无法精确建模，如果只按照处理方程来进行计算，那么每次计算就会对误差进行 A A A倍的放大，会越来越不准，所以需要引入测量量和系统噪声， ϵ t \epsilon_t ϵt​就是状态方程的噪声

测量方程

测量方程指的是系统 t t t时刻状态 x t x_t xt​映射出来的一些特征 z t z_t zt​

z k = H x k + ν \LARGE z_k=Hx_k+\nu zk​=Hxk​+ν
测量方程中的 x k x_k xk​同状态方程中的 x k x_k xk​， ν \nu ν是测量噪声，没有百分百能够测得准的传感器， z k z_k zk​是测量值， H H H是映射矩阵，把状态值变化为测量值
回到我们的例子里面，假设我们在起点处放置了一个传感器，该传感器可以测得位移大小，那么我们的测量方程就是
z k = [ 1 0 ] ∗ [ S k V k ] z_k=\begin{bmatrix}1&0\end{bmatrix}*\begin{bmatrix}S_k\\V_k\end{bmatrix} zk​=[1​0​]∗[Sk​Vk​​]
注：此处没有写出噪声 那么 H = [ 1 0 ] H=\begin{bmatrix}1&0\end{bmatrix} H=[1​0​]
对于测量方程，其实测量值 z z z是我们通过传感器去测量得到的，那么这个方程由什么作用呢，这个方程的作用就是带入我们在状态方程中求的 x k x_k xk​，称为预测值，记为 x k ^ \hat{x_k} xk​^​，然后计算 x k ^ \hat{x_k} xk​^​的测量值 z k z_k zk​，记为 z k ^ \hat{z_k} zk​^​，再和实际情况下通过测量得到的 z t z_t zt​进行比较，继而进行融合

方程小汇总

此处对上述所提到的方程进行一个小的汇总

处理方程

x k = A x k − 1 + B u k + ϵ \LARGE x_k=Ax_{k-1}+Bu_k+\epsilon xk​=Axk−1​+Buk​+ϵ

测量方程

z k = H x k + ν \LARGE z_k=Hx_k+\nu zk​=Hxk​+ν

例

按照匀加速运动的例子

处理方程

[ S k V k ] = [ 1 Δ t 0 1 ] ∗ [ S k − 1 V k − 1 ] + [ 1 2 a ( Δ t ) 2 Δ t ] ∗ a + ϵ \LARGE \begin{bmatrix} S_{k}\\V_{k} \end{bmatrix}\quad =\begin{bmatrix}1&\Delta t\\0&1\end{bmatrix}*\begin{bmatrix}S_{k-1}\\V_{k-1}\end{bmatrix} +\begin{bmatrix}\frac{1}{2}a(\Delta t)^2\\\Delta t\end{bmatrix}*a+\epsilon ⎣⎢⎡​Sk​Vk​​⎦⎥⎤​=⎣⎢⎡​10​Δt1​⎦⎥⎤​∗⎣⎢⎡​Sk−1​Vk−1​​⎦⎥⎤​+⎣⎢⎡​21​a(Δt)2Δt​⎦⎥⎤​∗a+ϵ
其中 x k = [ S k V k ] x_k=\begin{bmatrix} S_{k}\\V_{k} \end{bmatrix} xk​=[Sk​Vk​​] A = [ 1 Δ t 0 1 ] A=\begin{bmatrix}1&\Delta t \\0&1\end{bmatrix} A=[10​Δt1​] B = [ 1 2 ( Δ t ) 2 Δ t ] B=\begin{bmatrix}\frac{1}{2}(\Delta t)^2\\\Delta t\end{bmatrix} B=[21​(Δt)2Δt​] u k = a u_k=a uk​=a ϵ \epsilon ϵ是处理方程的噪声

测量方程

z k = [ 1 0 ] ∗ [ S k V k ] + ν \LARGE z_k=\begin{bmatrix}1&0\end{bmatrix}*\begin{bmatrix}S_k\\V_k\end{bmatrix}+\nu zk​=[1​0​]∗⎣⎢⎡​Sk​Vk​​⎦⎥⎤​+ν
其中 z k = [ S k ] z_k=\begin{bmatrix}S_k\end{bmatrix} zk​=[Sk​​] H = [ 1 0 ] H=\begin{bmatrix}1&0\end{bmatrix} H=[1​0​] ν \nu ν是测量过程的噪声

噪声处理

现在我们开始考虑两个方程中噪声的问题
对于 ϵ \epsilon ϵ和 ν \nu ν假设这两个噪声服从如下的多元高斯分布
p ( ϵ ) ∼ N ( 0 , Q ) p ( ν ) ∼ N ( 0 , R ) \LARGE p(\epsilon)\thicksim N(0,Q)\\p(\nu)\thicksim N(0,R) p(ϵ)∼N(0,Q)p(ν)∼N(0,R)
为什么噪声应当服从高斯分布呢？继续往下
我们假设系统噪声只位于速度分量上，且速度噪声的方差是一个常量0.01，那么就有
Q = [ 0 0 0 0.01 ] Q=\begin{bmatrix}0&0\\0&0.01\end{bmatrix} Q=[00​00.01​]
这个Q表示的意思是，在模型的处理方程中，在速度上系统噪声方差为0.01，位移上的为0，二者的协方差为零，说明二者的噪声独立，互不相关

预测与更新

有了之前的说明，那么我们就要进行数据的融合工作，即通过预测值和测量值得到一个最优的估计值

定义几个值

x k ′ ^ \hat{x'_k} xk′​^​是预测（先验）值，即使用状态方程计算出来的值
x k ^ \hat{x_k} xk​^​是估计值，即用预测值和测量值融合得到的结果
z k ^ \hat{z_k} zk​^​是预测测量值，即把 x k ′ ^ \hat{x'_k} xk′​^​带入测量方程计算的预测测量值
z k z_k zk​是实际的测量值

估计值计算方程

x k ^ = x k ′ ^ + K k ( z k − z k ^ ) = x k ′ ^ + K k ( z k − H ( x k ′ ^ ) ) \huge \hat{x_k}=\hat{x'_k}+K_k(z_k-\hat{z_k})\\ \qquad \quad=\hat{x'_k}+K_k(z_k-H(\hat{x'_k})) xk​^​=xk′​^​+Kk​(zk​−zk​^​)=xk′​^​+Kk​(zk​−H(xk′​^​))
其中 z k − z k ^ = z k − H ( x k ′ ^ ) z_k-\hat{z_k}=z_k-H(\hat{x'_k}) zk​−zk​^​=zk​−H(xk′​^​) 这个东西称为残差，其实就是实际测量值与预测测量值的差

对于这个方程，我们可以从特殊情况下来看待理解它。
如果残差为零，那么说明真实测量与预测测量没有差别，那么
x k ^ = x k ′ ^ + K k ∗ 0 ⃗ 即 x k ^ = x k ′ ^ \huge \hat{x_k}=\hat{x'_k}+K_k*\vec0\\即\hat{x_k}=\hat{x'_k} xk​^​=xk′​^​+Kk​∗0 即xk​^​=xk′​^​那么说明预测值与估计值一样
如果 K k = 0 K_k=0 Kk​=0，说明我们就不考虑测量值对于系统状态的影响，我们只信赖状态方程估算的数据，那么
x k ^ = x k ′ ^ + 0 ⃗ ∗ ( z k − z k ^ ) 即 x k ^ = x k ′ ^ \huge \hat{x_k}=\hat{x'_k}+\vec0*(z_k-\hat{z_k})\\即\hat{x_k}=\hat{x'_k} xk​^​=xk′​^​+0 ∗(zk​−zk​^​)即xk​^​=xk′​^​
如果 K k = 1 K_k=1 Kk​=1，说明我们非常信赖测量的数据，那么
x k ^ = x k ′ ^ + 1 ⃗ ∗ ( z k − z k ^ ) = x k ′ ^ + z k − z k ^ = x k ′ ^ + z k − H x k ′ ^ \huge \quad\hat{x_k}=\hat{x'_k}+\vec1*(z_k-\hat{z_k})\\ =\hat{x'_k}+z_k-\hat{z_k}\\ \quad=\hat{x'_k}+z_k-H\hat{x'_k}\\ xk​^​=xk′​^​+1 ∗(zk​−zk​^​)=xk′​^​+zk​−zk​^​=xk′​^​+zk​−Hxk′​^​
根据匀加速直线运动的例子我们继续展开上式
[ S k ^ V k ^ ] = [ S k ′ ^ V k ′ ^ ] + [ S k 测 0 ] − [ 1 0 ] ∗ [ S k ′ ^ V k ′ ^ ] = [ S k 测 V k ′ ^ ] \huge\begin{bmatrix}\hat{S_k}\\\hat{V_k}\end{bmatrix}=\begin{bmatrix}\hat{S'_k}\\\hat{V'_k}\end{bmatrix}+\begin{bmatrix}S_{k测}\\0\end{bmatrix}-\begin{bmatrix}1&0\end{bmatrix}*\begin{bmatrix}\hat{S'_k}\\\hat{V'_k}\end{bmatrix}=\begin{bmatrix}S_{k测}\\\hat{V'_k}\end{bmatrix} ⎣⎢⎢⎢⎡​Sk​^​Vk​^​​⎦⎥⎥⎥⎤​=⎣⎢⎢⎢⎡​Sk′​^​Vk′​^​​⎦⎥⎥⎥⎤​+⎣⎢⎢⎡​Sk测​0​⎦⎥⎥⎤​−[1​0​]∗⎣⎢⎢⎢⎡​Sk′​^​Vk′​^​​⎦⎥⎥⎥⎤​=⎣⎢⎢⎡​Sk测​Vk′​^​​⎦⎥⎥⎤​
由此可以看出，当 K k = 1 K_k=1 Kk​=1时，我们对于测量方程的测量结果完全信任，对于能够测量到的数据，一律认为是我们的估计值
所以对于估计值计算方程。
我们可以理解为 K k K_k Kk​是残差的系数，我们融合的就是预测值和残差，关键就在于如何求解这个 K k \huge K_k Kk​

残差系数的求解

估计值和真实值的差距

P k = E [ e k e k T ] = E [ ( x k − x k ^ ) ( x k − x k ^ ) T ] P_k = E[e_ke^T_k]=E[(x_k-\hat{x_k})(x_k-\hat{x_k})^T] Pk​=E[ek​ekT​]=E[(xk​−xk​^​)(xk​−xk​^​)T]
这个是真实值与估计值的协防差矩阵
e k e_k ek​是一个向量，它是由系统状态的变量组成的，在例子中就是由 S S S和 V V V组成，即位移和速度。那么就有

P k = [ E ( S e r r S e r r T ) E ( S e r r V e r r T ) E ( V e r r S e r r T ) E ( V e r r V e r r T ) ] P_k=\begin{bmatrix}E(S_{err}S^T_{err}) &E(S_{err}V^T_{err})\\E(V_{err}S^T_{err}) &E(V_{err}V^T_{err})\end{bmatrix} Pk​=[E(Serr​SerrT​)E(Verr​SerrT​)​E(Serr​VerrT​)E(Verr​VerrT​)​]

S e r r S_{err} Serr​是位移的误差， V e r r V_{err} Verr​是速度的误差，对角线上是他们各自的方差

x k − x k ^ = x k − ( x k ′ ^ + K k ( z k − z k ^ ) )       = x k − ( x k ′ ^ + K k ( H k x k + ν k − H k x k ′ ^ ) )       = x k − x k ′ ^ − K k H k x k − K k ν k + K k H k x k ′ ^       = ( I − K k H k ) ( x k − x k ′ ^ ) − K k ν k x_k-\hat{x_k}=x_k-(\hat {x'_k}+K_k(z_k-\hat{z_k})) \\\qquad\quad\;\;\, =x_k-(\hat {x'_k}+K_k(H_kx_k+\nu_k-H_k\hat{x'_k})) \\\qquad\quad\;\;\,=x_k-\hat{x'_k}-K_kH_kx_k-K_k\nu_k+K_kH_k\hat{x'_k} \\\qquad\quad\;\;\,=(I-K_kH_k)(x_k-\hat{x'_k})-K_k\nu_k xk​−xk​^​=xk​−(xk′​^​+Kk​(zk​−zk​^​))=xk​−(xk′​^​+Kk​(Hk​xk​+νk​−Hk​xk′​^​))=xk​−xk′​^​−Kk​Hk​xk​−Kk​νk​+Kk​Hk​xk′​^​=(I−Kk​Hk​)(xk​−xk′​^​)−Kk​νk​

此处 z k ^ = H k x k ′ ^ \hat{z_k}=H_k\hat{x'_k} zk​^​=Hk​xk′​^​，为什么没有噪声呢，博主自己理解的是，因为这个值不是真实测量的所以不引入噪声。就算如果引入了噪声，那么根据 z k − z k ^ z_k-\hat{z_k} zk​−zk​^​，噪声就被消去了，噪声就没用了。博主认为应该似乎就是这种原因吧。

把 x k − x k ^ x_k-\hat{x_k} xk​−xk​^​的表达式代入，直接展开,又因为 x k x_k xk​与 ν k \nu_k νk​是相互独立的,所以有 E [ A ν k ] = E [ A ] E [ ν k ] E[A\nu_k]=E[A]E[\nu_k] E[Aνk​]=E[A]E[νk​]， A A A是一个表达式，且 E [ ν k ] = 0 E[\nu_k]=0 E[νk​]=0，所以只带有 ν k \nu_k νk​的期望项都为零
P k = E [ [ ( I − K k H k ) ( x k − x k ′ ^ ) − K k ν k ] [ ( I − K k H k ) ( x k − x k ′ ^ ) − K k ν k ] T ]     = E [ [ ( I − K k H k ) ( x k − x k ′ ^ ) − K k ν k ] [ ( x − x k ′ ^ ) T ( I − K k H k ) T − ν k T K k T ] ]     = E [ ( I − K k H k ) ( x k − x k ′ ^ ) ( x k − x k ′ ^ ) T ( I − K k H k ) T + K k ν k ν k T K k T ]     = ( I − K k H k ) E [ ( x k − x k ′ ^ ) ( x k − x k ′ ^ ) T ] ( I − K k H k ) T + K k E [ ν k ν k T ] K k T P_k=E\begin{bmatrix}[(I-K_kH_k)(x_k-\hat{x'_k})-K_k\nu_k][(I-K_kH_k)(x_k-\hat{x'_k})-K_k\nu_k]^T\end{bmatrix} \\\quad\;\,=E\begin{bmatrix}[(I-K_kH_k)(x_k-\hat{x'_k})-K_k\nu_k][(x-\hat{x'_k})^T(I-K_kH_k)^T-\nu_k^TK_k^T]\end{bmatrix} \\\quad\;\,=E\begin{bmatrix}(I-K_kH_k)(x_k-\hat{x'_k})(x_k-\hat{x'_k})^T(I-K_kH_k)^T+K_k\nu_k\nu_k^TK_k^T\end{bmatrix} \\\quad\;\,=(I-K_kH_k)E\begin{bmatrix}(x_k-\hat{x'_k})(x_k-\hat{x'_k})^T\end{bmatrix}(I-K_kH_k)^T+K_kE\begin{bmatrix}\nu_k\nu_k^T\end{bmatrix}K_k^T Pk​=E[[(I−Kk​Hk​)(xk​−xk′​^​)−Kk​νk​][(I−Kk​Hk​)(xk​−xk′​^​)−Kk​νk​]T​]=E[[(I−Kk​Hk​)(xk​−xk′​^​)−Kk​νk​][(x−xk′​^​)T(I−Kk​Hk​)T−νkT​KkT​]​]=E[(I−Kk​Hk​)(xk​−xk′​^​)(xk​−xk′​^​)T(I−Kk​Hk​)T+Kk​νk​νkT​KkT​​]=(I−Kk​Hk​)E[(xk​−xk′​^​)(xk​−xk′​^​)T​](I−Kk​Hk​)T+Kk​E[νk​νkT​​]KkT​
又因为 E [ ν k ν k T ] = R E\begin{bmatrix}\nu_k\nu_k^T\end{bmatrix}=R E[νk​νkT​​]=R，所以就有
P k = ( I − K k H k ) E [ ( x k − x k ′ ^ ) ( x k − x k ′ ^ ) T ] ( I − K k H k ) T + K k R K k T P_k=(I-K_kH_k)E\begin{bmatrix}(x_k-\hat{x'_k})(x_k-\hat{x'_k})^T\end{bmatrix}(I-K_kH_k)^T+K_kRK_k^T Pk​=(I−Kk​Hk​)E[(xk​−xk′​^​)(xk​−xk′​^​)T​](I−Kk​Hk​)T+Kk​RKkT​
我们发现此时在 P k P_k Pk​中出现了一个期望 E [ ( x k − x k ′ ^ ) ( x k − x k ′ ^ ) T ] E\begin{bmatrix}(x_k-\hat{x'_k})(x_k-\hat{x'_k})^T\end{bmatrix} E[(xk​−xk′​^​)(xk​−xk′​^​)T​]，由前面可知，这是真实值与预测值的协方差矩阵
所以有 P k ′ = E [ ( x k − x k ′ ^ ) ( x k − x k ′ ^ ) T ] P'_k=E\begin{bmatrix}(x_k-\hat{x'_k})(x_k-\hat{x'_k})^T\end{bmatrix} Pk′​=E[(xk​−xk′​^​)(xk​−xk′​^​)T​]
P k ′ P'_k Pk′​是真实值与预测值的协方差矩阵
现在回到 P k P_k Pk​的表达式
P k = ( I − K k H k ) P k ′ ( I − K k H k ) T + K k R K k T     = P k ′ − K k H k P k ′ − P k ′ H k T K k T + K k ( H k P k ′ H k T + R ) K k T P_k=(I-K_kH_k)P'_k(I-K_kH_k)^T+K_kRK_k^T\\ \quad\;\,=P'_k-K_kH_kP'_k-P'_kH_k^TK_k^T+K_k(H_kP'_kH_k^T+R)K_k^T Pk​=(I−Kk​Hk​)Pk′​(I−Kk​Hk​)T+Kk​RKkT​=Pk′​−Kk​Hk​Pk′​−Pk′​HkT​KkT​+Kk​(Hk​Pk′​HkT​+R)KkT​
我们希望求到最小均方差，那么 P k P_k Pk​的迹（矩阵对角线元素之和，也是均方差）应该最小，所以
t r ( P k ) = t r ( P k ′ ) − t r ( K k H k P k ′ ) − t r ( P k ′ H k T K k T ) + t r [ K k ( H k P k ′ H k T + R ) K k T ]         = t r ( P k ′ ) − 2 t r ( K k H k P k ′ ) + t r [ K k ( H k P k ′ H k T + R ) K k T ] tr(P_k)=tr(P'_k)-tr(K_kH_kP'_k)-tr(P'_kH_k^TK_k^T)+tr[K_k(H_kP'_kH_k^T+R)K_k^T]\\ \qquad\;\;\;\,=tr(P'_k)-2tr(K_kH_kP'_k)+tr[K_k(H_kP'_kH_k^T+R)K_k^T] tr(Pk​)=tr(Pk′​)−tr(Kk​Hk​Pk′​)−tr(Pk′​HkT​KkT​)+tr[Kk​(Hk​Pk′​HkT​+R)KkT​]=tr(Pk′​)−2tr(Kk​Hk​Pk′​)+tr[Kk​(Hk​Pk′​HkT​+R)KkT​]
对 K k K_k Kk​求偏导，此处需要用到两个矩阵微分公式
公式一：
∂ t r ( A B ) ∂ A = B T \frac{\partial tr(AB)}{\partial A}=B^T ∂A∂tr(AB)​=BT
公式二：
∂ t r ( A B A T ) ∂ A = 2 A B \frac{\partial tr(ABA^T)}{\partial A}=2AB ∂A∂tr(ABAT)​=2AB
∂ t r ( P k ) ∂ K k     = − ∂ 2 t r ( K k H k P k ′ ) ∂ K k + ∂ t r [ K k ( H k P k ′ H k T + R ) K k T ] ∂ K k = − 2 ( H k P k ′ ) T + 2 K k ( H P k ′ H T + R ) \frac{\partial tr(P_k)}{\partial K_k}\;\,=-\frac{\partial2tr(K_kH_kP'_k)}{\partial K_k}+\frac{\partial tr[K_k(H_kP'_kH_k^T+R)K_k^T]}{\partial K_k}\\=-2(H_kP'_k)^T+2K_k(HP'_kH^T+R) ∂Kk​∂tr(Pk​)​=−∂Kk​∂2tr(Kk​Hk​Pk′​)​+∂Kk​∂tr[Kk​(Hk​Pk′​HkT​+R)KkT​]​=−2(Hk​Pk′​)T+2Kk​(HPk′​HT+R)
为了求得最小值，偏导应该为零，对于协方差矩阵 P = P T P=P^T P=PT
0 = − 2 ( H k P k ′ ) T + 2 K k ( H P k ′ H T + R ) 0=-2(H_kP'_k)^T+2K_k(HP'_kH^T+R) 0=−2(Hk​Pk′​)T+2Kk​(HPk′​HT+R)
整理可得

卡尔曼增益的表达式

K k = P k ′ H k T ( H P k ′ H T + R ) − \huge K_k=P'_kH_k^T(HP'_kH^T+R)^- Kk​=Pk′​HkT​(HPk′​HT+R)−
得到了 K k K_k Kk​的计算方法，只需求出 P k ′ P_k' Pk′​(真实值与预测值的协方差矩阵)即可

预测值和真实值的差距

P k ′ = E [ ( x k − x k ′ ^ ) ( x k − x k ′ ^ ) T ]    = E [ ( x k − x k ′ ^ ) ( x k − x k ′ ^ ) T ]    = E [ ( A k x k + B k u k + ϵ k − A k x ^ k − 1 − B k u k ) ( A k x k + B k u k + ϵ k − A k x ^ k − 1 − B k u k ) T ]    = E [ ( A k ( x k − x ^ k − 1 ) + ϵ k ) ( A k ( x k − x ^ k − 1 ) + ϵ k ) T ]    = E [ ( A k ( x k − x ^ k − 1 ) ( A k ( x k − x ^ k − 1 ) T ] + E [ ϵ k ϵ k T ] P'_k=E\begin{bmatrix}(x_k-\hat{x'_k})(x_k-\hat{x'_k})^T\end{bmatrix}\\ \quad\;=E\begin{bmatrix}(x_k-\hat{x'_k})(x_k-\hat{x'_k})^T\end{bmatrix}\\ \quad\;=E\begin{bmatrix}(A_kx_k+B_ku_k+\epsilon_k-A_k\hat{x}_{k-1}-B_ku_k)(A_kx_k+B_ku_k+\epsilon_k-A_k\hat{x}_{k-1}-B_ku_k)^T\end{bmatrix}\\ \quad\;=E\begin{bmatrix}(A_k(x_k-\hat{x}_{k-1})+\epsilon_k)(A_k(x_k-\hat{x}_{k-1})+\epsilon_k)^T\end{bmatrix}\\ \quad\;=E\begin{bmatrix}(A_k(x_k-\hat{x}_{k-1})(A_k(x_k-\hat{x}_{k-1})^T\end{bmatrix}+E\begin{bmatrix}\epsilon_k\epsilon_k^T\end{bmatrix} Pk′​=E[(xk​−xk′​^​)(xk​−xk′​^​)T​]=E[(xk​−xk′​^​)(xk​−xk′​^​)T​]=E[(Ak​xk​+Bk​uk​+ϵk​−Ak​x^k−1​−Bk​uk​)(Ak​xk​+Bk​uk​+ϵk​−Ak​x^k−1​−Bk​uk​)T​]=E[(Ak​(xk​−x^k−1​)+ϵk​)(Ak​(xk​−x^k−1​)+ϵk​)T​]=E[(Ak​(xk​−x^k−1​)(Ak​(xk​−x^k−1​)T​]+E[ϵk​ϵkT​​]
     = A k P k − 1 A k T + Q \huge\;\;=A_kP_{k-1}A_k^T+Q =Ak​Pk−1​AkT​+Q

使用卡尔曼增益去更新估计值与真实值的差距

把卡尔曼增益表达式代入 P k P_k Pk​表达式
K k = P k ′ H k T ( H P k ′ H T + R ) − K_k=P'_kH_k^T(HP'_kH^T+R)^- Kk​=Pk′​HkT​(HPk′​HT+R)−
P k = P k ′ − K k H k P k ′ − P k ′ H k T K k T + K k ( H k P k ′ H k T + R ) K k T P_k=P'_k-K_kH_kP'_k-P'_kH_k^TK_k^T+K_k(H_kP'_kH_k^T+R)K_k^T Pk​=Pk′​−Kk​Hk​Pk′​−Pk′​HkT​KkT​+Kk​(Hk​Pk′​HkT​+R)KkT​
注意仅带入到第三项的 K k K_k Kk​中
P k = P k ′ − K k H k P k ′ − P k ′ H k T K k T + P k ′ H k T ( H P k ′ H T + R ) − ( H k P k ′ H k T + R ) K k T P_k=P'_k-K_kH_kP'_k-P'_kH_k^TK_k^T+P'_kH_k^T(HP'_kH^T+R)^-(H_kP'_kH_k^T+R)K_k^T Pk​=Pk′​−Kk​Hk​Pk′​−Pk′​HkT​KkT​+Pk′​HkT​(HPk′​HT+R)−(Hk​Pk′​HkT​+R)KkT​
    = P k ′ − K k H k P k ′ \quad\;\,=P'_k-K_kH_kP'_k =Pk′​−Kk​Hk​Pk′​
    = ( I − K k H k ) P k ′ \quad\;\,\huge=(I-K_kH_k)P'_k =(I−Kk​Hk​)Pk′​

总结

卡尔曼滤波的更新过程

1， P 0 P_0 P0​和 x 0 x_0 x0​是已知量，由 P 0 P_0 P0​计算出 P 1 ′ P'_1 P1′​，再算出 K 1 K_1 K1​，结合测量值算出 x 1 x_1 x1​，再利用 K 1 K_1 K1​更新 P 1 P_1 P1​
2， 由 P 1 P_1 P1​计算出 P 2 ′ P'_2 P2′​，再算出 K 2 K_2 K2​，结合测量值算出 x 2 x_2 x2​，再利用 K 2 K_2 K2​更新 P 2 P_2 P2​
。。。。。。
n，由 P n − 1 P_{n-1} Pn−1​计算出 P n ′ P'_n Pn′​，再算出 K n K_n Kn​，结合测量值算出 x n x_n xn​，再利用 K n K_n Kn​更新 P n P_n Pn​

需要使用的公式

x k ′ ^     = A x ^ k − 1 + B u k \huge \hat{x'_k}\;\,=A\hat{x}_{k-1}+Bu_k xk′​^​=Ax^k−1​+Buk​
P k ′   = A P k − 1 A T + Q \huge P'_k\,=AP_{k-1}A^T+Q Pk′​=APk−1​AT+Q
K k = P k ′ H T ( H P k ′ H T + R ) − \huge K_k=P'_kH^T(HP'_kH^T+R)^- Kk​=Pk′​HT(HPk′​HT+R)−
z k ^      = H x k ′ ^ + ν \huge \hat{z_k}\;\;=H\hat{x'_k}+\nu zk​^​=Hxk′​^​+ν
x k ^ = x k ′ ^ + K k ( z k − z k ^ ) \huge \hat{x_k}=\hat{x'_k}+K_k(z_k-\hat{z_k}) xk​^​=xk′​^​+Kk​(zk​−zk​^​)
P k    = ( I − K k H ) P k ′ \huge P_k\;=(I-K_kH)P'_k Pk​=(I−Kk​H)Pk′​
其中：
p ( ϵ ) ∼ N ( 0 , Q ) p ( ν ) ∼ N ( 0 , R ) \huge p(\epsilon)\thicksim N(0,Q)\\p(\nu)\thicksim N(0,R) p(ϵ)∼N(0,Q)p(ν)∼N(0,R)

例子

一辆匀速直线运动的车，从原点出发 S 0 = 0 m S_0=0m S0​=0m，初始速度 V 0 = 0 m / s V_0=0m/s V0​=0m/s求出车与原点的位移，加速度 a = 2 m / s a=2m/s a=2m/s，每次的测量时间间隔 Δ t = 0.2 s \Delta t=0.2s Δt=0.2s， Q = 0.02 Q=0.02 Q=0.02， R = 10 R=10 R=10，对于 P k ′ P'_k Pk′​的初值自己设定，本例中设为 [ 0 0 0 0 ] \begin{bmatrix}0&0\\0&0\end{bmatrix} [00​00​]，噪声也是自己设定的

import numpy as np
import matplotlib as mlp
import matplotlib.pyplot as plt

#预测方程噪声的协方差矩阵
Q=np.array([(0,0),(0,0.02)])
#测量方程噪声的方差
R=np.array([10]);
#采样时间
deltaT = 0.1
#生成所有的时间
t=np.arange(0,5,deltaT)
N=t.size
#加速度
a = 2
#真实位移
x=1/2*a*t*t
np.random.seed(0)
noise = np.random.normal(0,np.sqrt(10),N)
#生成测量数据
z=x+noise
#A,B,H矩阵
A=np.array([(1,deltaT),(0,1)])
B=np.array([(1/2*deltaT*deltaT),(deltaT)])
H=np.array([(1,0)])
#估计值
xhat=np.zeros([2,t.shape[0]])
#预测值
xhatminus=np.zeros([2,t.shape[0]])
#估计值和真实值协方差矩阵
P=np.zeros(Q.shape)
#预测值和真实值协方差矩阵
Pminus=np.zeros(Q.shape)
#单位矩阵
I=np.eye(Q.shape[0])

#进行卡尔曼滤波
for k in range(9,N):
    xhatminus[:,k]=np.dot(A,xhat[:,k-1])+np.dot(B,a)
    Pminus = np.dot(np.dot(A,P),A.T)+Q   
    K=np.dot(np.dot(Pminus,H.T),np.linalg.inv(np.dot(np.dot(H,Pminus),H.T)+R)) 
    xhat[:,k]=xhatminus[:,k]+ np.dot(K,z[k]-np.dot(H,xhatminus[:,k]))
    P=np.dot((I-np.dot(K,H)),Pminus)

plt.rcParams['font.sans-serif']=['Simhei'] 
plt.plot(z,'r',label=u'测量数据')
plt.plot(x,'g',label=u'真值')
plt.plot(xhat[0,:],'b',label=u'估计值')
plt.legend(loc = 0)
plt.show()

效果图
参考：
卡尔曼滤波 – 从推导到应用(一)
卡尔曼滤波 – 从推导到应用(二)
无人驾驶汽车系统入门（一）——卡尔曼滤波与目标追踪
卡尔曼滤波算法详细推导

cmf8797143
关注
  • 0
    点赞
  • 踩
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
无损卡尔曼滤波学习笔记
申申的博客
04-04 2800
作为学习自动驾驶的一部分，在这里把学习无损卡尔曼滤波的过程记录一下，以下内容会有很多都是参考别的博客，下面也会给出链接： 无损卡尔曼滤波UKF的原理 参考链接：https://blog.csdn.net/lijil168/article/details/83315595 从前面学习卡尔曼滤波，及扩展卡尔曼滤波的过程来看，KF的核心原理是根据系统的估算值及协方差，...
MOT学习 - 卡尔曼滤波
tsq292978891的博客
05-07 1010
参考资料： https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/ What is it? 使用场景：对于某个动态系统有不确定的信息，需要对系统接下来要做什么做出有根据的猜测。卡尔曼滤波器非常适合不断变化的系统。 鲁棒性：即使混乱的现实出现并干扰了您猜测的清晰运动，卡尔曼滤波器通常也能很好地找出实际发生的情况。它可以利用您可能不会想到利用的疯狂现象之间的相关性！ 实时性：优点是内存占用少（除了以前的状态之外不需要保留任何历史记录），而
卡尔曼滤波学习笔记
weixin_45885074的博客
08-22 328
Kalman Filter....
卡尔曼滤波学习笔记——欢迎指正
weixin_55255438的博客
12-14 724
个人对矩阵浅显的理解，规定了矩阵（Matrix）是一个按照长方阵列排列的或集合。再规定两个这种集合间可以进行加减乘除，最后规定加减乘除是怎么个加减乘除。以此基础推演发散高出各种花样。可以通过计算得到过段时间的状态，但是这是一种推测。
随机过程——卡尔曼滤波学习笔记
m0_46521579的博客
01-11 594
V(k)基于样本观测的条件均值为0，即均方不可预测。
【学习笔记】卡尔曼滤波 Kalman Filter
一些笔记
02-21 1946
对卡尔曼滤波算法的系统学习
卡尔曼滤波学习(4)
lglrkx的博客
03-09 383
去得到最优解，就和之前 卡尔曼学习(2) 中根据两个有误差结果去估计一个相对准确的最优解很相似，不过是换做了一个预测值和一个测量值去进行数据融合。在之前的 卡尔曼学习(1) 中提到过，系统的估计和测量都不可避免的存在误差，在方程(5)和(6)中分别引入过程噪音。上述方程(1)和(2)为该系统的状态变化方程，体现了该系统物体的速度和加速度随时间的变化量。对时间的导数，反应出在连续时间上的变化，即系统状态变化。关于t时刻的二阶导数，即t时刻的加速度。关于t时刻的导数，即t时刻的速度；的观测值，即测量值。
学习记录-卡尔曼滤波推导
Usitin_ian的博客
04-12 197
singleDoc# 《卡尔曼滤波》很简洁明了，我算是挨个推了一遍，补充了一些基础内容还有推理过程。啊，这只是我的一个记录，纪念一下我推了半天的卡尔曼滤波。
卡尔曼滤波学习-01
m0_54407102的博客
01-24 294
卡尔曼滤波的初步学习和认识
【卡尔曼滤波的学习，以及一些理解】
qq_40185784的博客
05-22 1371
卡尔曼滤波是组合不同传感器比较核心的算法，应用也比较广泛，也有很多文章对其进行了详细的介绍，这里记录一下自己的一些理解。
卡尔曼滤波算法C语言实现.pdf
04-03
详细讲解了卡尔曼滤波算法的每一个步骤，附有C语言代码实现，有部分笔记，以及笔记笔录，认真看应该能看懂
kalman-卡尔曼滤波学习笔记.pdf
05-04
本文内容部分是基于参考链接进行的摘抄和编辑，为的是加强自己的阅读效果，留下阅读的笔记，内容做过深度阅读和改动，参照部分版权属于原作者所有。
卡尔曼滤波/卡尔曼预测-个人学习笔记
12-03
通过笔记1和笔记2的深入学习，可以进一步理解卡尔曼滤波的细节，例如状态转移矩阵、观测矩阵的构造，以及在非线性系统中的扩展——扩展卡尔曼滤波和无迹卡尔曼滤波。同时，卡尔曼作业.xlsx可能包含了一些练习题目和...
2024钉钉杯A题思路详解
最新发布
全栈川川
07-26 1665
2024钉钉杯思路
常见的文心一言的指令
知码客
07-25 1332
文心一言，作为百度研发的预训练语言模型“ERNIE 3.0”的一项功能，能够与人对话互动，回答问题，协助创作，高效便捷地帮助人们获取信息、知识和灵感。
安装TensorRT各自小库版本问题
Ppandaer的博客
07-24 459
降版本：protobuf3.19，onnx 1.12。
全面解析 SnowNLP：中文文本处理、情感分析
有勇气的牛排博客
07-24 587
SnowNLP 是一个专门用于处理中文文本的 Python库。分词情感分析关键词提取文本分类拼音转换繁体转简体词相似度计算等测试环境：Python3.10.9尚未测出该功能text = "有勇气的牛排写的文章通俗易懂，爱了爱了"文本分类使用的是 SnowNLP 的情感分析模型。
AI智能技术驱动安全生产：智慧矿山视频智能监管新篇章
TSINGSEE青犀视频官方技术博客
07-26 688
当识别到有人员未按照要求穿戴安全帽、反光背心/工服时，将立即抓拍，并将告警信息上传到平台。
卡尔曼滤波详解：从最小二乘到无迹变换
"这是一份关于卡尔曼滤波的自学笔记和汇报PPT，主要涵盖了卡尔曼滤波算法的基本概念、最小二乘法及其加权估计、线性最小二乘递推估计、以及非线性系统的扩展卡尔曼滤波和无迹卡尔曼滤波方法。" 卡尔曼滤波是一种...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值