本文主要是介绍卡尔曼滤波公式通俗理解，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
本文需要配合博客卡尔曼滤波详解进行理解
1.简单介绍
参考卡尔曼滤波详解
上面可简化理解为
2.主要过程
主要过程还是参考卡尔曼滤波详解
3.实例
这里以线性运动为例
3.1 前期定义状态和变量
3.1.1分析运动情况
已知线性运动上一状态和当前状态的关系，假设没有噪声干扰，为
{ x ′ = x + v x Δ t y ′ = y + v y Δ t \begin{cases} x'=x+v_x \Delta t \\ y'=y+v_y \Delta t \\ \end{cases} {x′=x+vxΔty′=y+vyΔt
3.1.2 定义跟踪状态和状态转移矩阵
[ x y v x v y ] \left[ \begin{matrix} x \\ y \\ v_x \\ v_y \\ \end{matrix} \right] ⎣⎢⎢⎡xyvxvy⎦⎥⎥⎤
则根据3.1.1可以得到状态转移关系
[ x ′ y ′ v x ′ v y ′ ] = [ 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ] [ x y v x v y ] \left[ \begin{matrix} x' \\ y' \\ v_x' \\ v_y' \\ \end{matrix} \right]= \left[ \begin{matrix} 1 & 0 & \Delta t & 0\\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} x \\ y \\ v_x \\ v_y \\ \end{matrix} \right] ⎣⎢⎢⎡x′y′vx′vy′⎦⎥⎥⎤=⎣⎢⎢⎡10000100Δt0100Δt01⎦⎥⎥⎤⎣⎢⎢⎡xyvxvy⎦⎥⎥⎤
即
A = [ 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ] A=\left[ \begin{matrix} 1 & 0 & \Delta t & 0\\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] A=⎣⎢⎢⎡10000100Δt0100Δt01⎦⎥⎥⎤
加上状态转移噪声
x k = A x k − 1 + w x_k=Ax_{k-1}+w xk=Axk−1+w
3.1.3 定义状态到观测转移矩阵
由于在实际过程中，我们只观测到位置 [ z x z y ] \left[ \begin{matrix} z_x \\ z_y \\ \end{matrix} \right] [zxzy]所以我们要得到从跟踪状态 [ x y v x v y ] \left[ \begin{matrix} x \\ y \\ v_x \\ v_y \\ \end{matrix} \right] ⎣⎢⎢⎡xyvxvy⎦⎥⎥⎤到 [ z x z y ] \left[ \begin{matrix} z_x \\ z_y \\ \end{matrix} \right] [zxzy]的转换矩阵，假设我们观察的位置没有噪声干扰则：
[ z x z y ] = [ 1 0 0 0 0 1 0 0 ] [ x y v x v y ] \left[ \begin{matrix} z_x \\ z_y \\ \end{matrix} \right]= \left[ \begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \end{matrix} \right] \left[ \begin{matrix} x \\ y \\ v_x \\ v_y \\ \end{matrix} \right] [zxzy]=[10010000]⎣⎢⎢⎡xyvxvy⎦⎥⎥⎤
即
H = [ 1 0 0 0 0 1 0 0 ] H=\left[ \begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \end{matrix} \right] H=[10010000]
加上测量噪声可得到
z k = H x k + v z_k=Hx_k+v zk=Hxk+v
由上面三部我们就基本把所需要的变量定义完了，下面就需要进行预测和矫正了
3.4 卡尔曼滤波过程
假设我们来了一帧数据
[ z x z y t ] \left[ \begin{matrix} z_x \\ z_y \\ t \\ \end{matrix} \right] ⎣⎡zxzyt⎦⎤
其中 [ z x z y ] \left[ \begin{matrix} z_x \\ z_y \\ \end{matrix} \right] [zxzy]为观测数据， t t t为当前时间，则进行下面步骤：
- (预测)根据 t t t和上一帧位置，预测当前帧位置，同时计算预测误差 P k ‾ \overline{P_k} Pk
- (矫正)求出卡尔曼增益，计算最优位置估计，计算当前估计误差 P k P_k Pk
3.2.1 预测
求出 Δ t = t − t p r e \Delta t=t-t_{pre} Δt=t−tpre，根据3.1.1估算当前位置
[ x ′ y ′ v x ′ v y ′ ] = [ 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ] [ x y v x v y ] \left[ \begin{matrix} x' \\ y' \\ v_x' \\ v_y' \\ \end{matrix} \right]= \left[ \begin{matrix} 1 & 0 & \Delta t & 0\\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} x \\ y \\ v_x \\ v_y \\ \end{matrix} \right] ⎣⎢⎢⎡x′y′vx′vy′⎦⎥⎥⎤=⎣⎢⎢⎡10000100Δt0100Δt01⎦⎥⎥⎤⎣⎢⎢⎡xyvxvy⎦⎥⎥⎤
即 x k ^ ‾ = A x k − 1 ^ \overline{\hat{x_k}}=A\hat{x_{k-1}} xk^=Axk−1^
计算当前估计误差，结合基本公式(过程噪声 Q Q Q)
P k ‾ = A P k − 1 A T + Q \overline{P_k}=AP_{k-1}A^T+Q Pk=APk−1AT+Q
3.2.2 矫正
结合基本公式
计算卡尔曼增益(测量噪声 R R R)
K k = P k ‾ H T ( H P k ‾ H T + R ) − 1 K_k=\overline{P_k}H^T(H\overline{P_k}H^T+R)^{-1} Kk=PkHT(HPkHT+R)−1
计算最优位置估计
x k ^ = x k ^ ‾ + K k ( z k − H x k ^ ‾ ) \hat{x_k}=\overline{\hat{x_k}}+K_k(z_k-H\overline{\hat{x_k}}) xk^=xk^+Kk(zk−Hxk^)
计算估计误差
P k = ( I − K k H ) P k ‾ P_k=(I-K_kH)\overline{P_k} Pk=(I−KkH)Pk
3.2.3 实际过程
综上，我们在实际过程中，只要设置好
- 跟踪变量 x x x
- 状态转移矩阵 A A A
- 状态到观测矩阵 H H H
- 状态转移噪声 Q Q Q
- 测量噪声 R R R
- 初始估计误差 P k P_k Pk(设置不能太小)
- 第一次需要设置 x k ^ \hat{x_k} xk^的值
就可以了
这篇关于卡尔曼滤波公式通俗理解的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
