卡尔曼滤波学习记录
前言
博主是做无人驾驶车辆建图与定位的,所以针对于车辆方面来浅显的学习。
这一部分是关于卡尔曼滤波器的理论部分,言简意赅,只写关键的东西。
卡尔曼滤波的核心就是预测与更新。预测一个理论值,再根据测量值去修正这个理论值。
预测的值称为预测值,修正后的值称为估计值。
推理过程
方程
卡尔曼滤波主要用于测量数据与推测数据的融合,浅显的解释就是,我用一个系统算了一个值,比如说车速,我还有一个传感器能够测一个车速,这两个速度值都存在着不同程度的误差,那么我就把他俩融合起来,求一个比较准确的速度。
处理方程
处理方程就是抽象实际情况中的系统状态,得到处理模型方程。
一般我们遇到的处理方程是这样:
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+21a(Δ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 ⎣⎢⎡SkVk⎦⎥⎤=⎣⎢⎡10Δt1⎦⎥⎤∗⎣⎢⎡Sk−1Vk−1⎦⎥⎤+⎣⎢⎡21a(Δt)2Δt⎦⎥⎤∗a
状态方程里面的 x k = [ S k V k ] x_k=\begin{bmatrix} S_{k}\\V_{k} \end{bmatrix} xk=[SkVk] 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=[10]∗[SkVk]
注:此处没有写出噪声 那么 H = [ 1 0 ] H=\begin{bmatrix}1&0\end{bmatrix} H=[10]
对于测量方程,其实测量值 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 ⎣⎢⎡SkVk⎦⎥⎤=⎣⎢⎡10Δt1⎦⎥⎤∗⎣⎢⎡Sk−1Vk−1⎦⎥⎤+⎣⎢⎡21a(Δt)2Δt⎦⎥⎤∗a+ϵ
其中 x k = [ S k V k ] x_k=\begin{bmatrix} S_{k}\\V_{k} \end{bmatrix} xk=[SkVk] 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=[10]∗⎣⎢⎡SkVk⎦⎥⎤+ν
其中 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=[10] ν \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=[0000.01]
这个Q表示的意思是,在模型的处理方程中,在速度上系统噪声方差为0.01,位移上的为0,二者的协方差为零,说明二者的噪声独立,互不相关
预测与更新
有了之前的说明,那么我们就要进行数据的融合工作,即通过预测值和测量值得到一个最优的估计值
定义几个值
x k ′ ^ \hat{x'_k} xk′^