卡尔曼滤波学习记录

前言

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

推理过程

方程

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

处理方程

处理方程就是抽象实际情况中的系统状态,得到处理模型方程。
一般我们遇到的处理方程是这样:
x k = A x k − 1 + B u k + ϵ \LARGE x_k=Ax_{k-1}+Bu_k+\epsilon xk=Axk1+Buk+ϵ
其中传递的意思就是,该系统 k k k时刻的状态 x k x_k xk k − 1 k-1 k1时刻的状态 x k − 1 x_{k-1} xk1与转换矩阵 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=Sk1+Vk1Δt+21a(Δt)2
V k = V k − 1 + a Δ t \LARGE V_k=V_{k-1}+a\Delta t Vk=Vk1+aΔt
S S S是位移, V V V是速度, a a a是加速度, Δ t \Delta t Δt是间隔时间,即 k k k时刻与 k − 1 k-1 k1时刻的时间差值
把这个方程组和状态方程进行类比,我们可以得到
[ 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Δt1Sk1Vk1+21a(Δt)2Δta
状态方程里面的 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=Axk1+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Δt1Sk1Vk1+21a(Δt)2Δta+ϵ
其中 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^

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值