卡尔曼运动模型公式推导CTRV+CTRA

原创

已于 2022-06-26 14:20:37 修改 · 7.3k 阅读

94 ·

CC 4.0 BY-SA版权

本文为博主原创文章，未经博主允许禁止转载。如有问题，欢迎指正！

文章标签：

#卡尔曼滤波算法 #自动驾驶

于 2021-04-20 16:12:14 首次发布

本文详细介绍了扩展卡尔曼滤波(EKF)中的CTRV和CTRA运动模型及其公式推导过程，包括状态方程、观测方程及雅各比矩阵的计算，并探讨了Q矩阵的设置方法。

#! https://zhuanlan.zhihu.com/p/389589611

卡尔曼运动模型公式推导CTRV+CTRA

主要是EKF的CTRV、CTRA两个运动模型的公式推导，以及困扰我很久的Q矩阵推导，一直不明白为什么要用 $Δt\Delta t$ 来设置Q矩阵。直接看公式会比较枯燥，建议推导一下。

1 扩展卡尔曼滤波EKF

解决非线性问题，如极坐标系的雷达，观测到的是径向距离和角度，这个时候的观测矩阵 $H$ 就是非线性的函数。（极坐标系可以转笛卡尔坐标系，Apollo融合跟踪里面对雷达物体用的是笛卡尔坐标系做KF）
EKF和KF的区别主要是状态转换模型和观测模型可以是非线性的，可以使用泰勒展开式替换为线性函数，两个协方差矩阵 $P$ 和 $H$ 要使用雅各比矩阵计算每个状态量的一阶偏导。

预测

$x^k∣k−1=f(xk−1,uk,0)\hat x_{k|k-1} = f(x_{k-1},u_k,0)$

$Pk∣k−1=JFPk−1∣k−1JFT+QkP_{k|k-1} = J_FP_{k-1|k-1}J^T_F + Q_k$

使用Jacobians矩阵更新模型

$JF=∂f∂x∣x^k−1∣k−1,ukJ_F = \frac {\partial f}{\partial x} |_{\hat x_{k-1|k-1}, u_k}$

$JH=∂h∂x∣x^k∣k−1J_H = \frac {\partial h}{\partial x} |_{\hat x_{k|k-1}}$

更新

$y^k=zk−h(x^k∣k−1,0)\hat y_k = z_k - h(\hat x_{k|k-1},0)$

$Sk=JHPk∣k−1HkT+RkS_k = J_H P_{k|k-1} H^T_k + R_k$

$Kk=Pk∣k−1JHTSk−1K_k = P_{k|k-1}J^T_H S_k^{-1}$

$x^k∣k=x^k∣k−1+Kky^\hat x_{k|k} = \hat x_{k|k-1} + K_k \hat y$

$P_{k|k} = (I - K_k J_H) P_{k|k-1}$

2 恒定转弯率和速度模型(Constant Turn Rate and Velocity,CTRV)

状态量为：

$\begin{bmatrix}x \\ y \\ v \\ \theta \\ \omega \end{bmatrix}$

状态方程为：

$Xk+1=Xk+[∫tktk+1v(t)cos(θ(t))∫tktk+1v(t)sin(θ(t))0wΔt0]X_{k+1} = X_k + \begin{bmatrix} {\int^{t_{k+1}}_{t_k} v(t)cos(\theta(t))} \\ {\int^{t_{k+1}}_{t_k} v(t)sin(\theta(t))} \\ 0 \\ w \Delta t \\ 0 \end{bmatrix}$

$v(t)=v_k$ 为常量,角速度 $w$ 也是常量，和时间无关。后面公式有点多，速度项可能有k下标，或者没有， $v_k,v$ 均是一个意思。
积分化简项为 $\neq 0:$

note：对于 $w = 0$ 时，以下的公式都是可以简化的，简化公式不做阐述，可自行推导。或者在代码中也可以使用 $w = 0.0001$ 一个极小数来赋值，解决被除无限大的问题

$∫tktk+1v(t)cos(θ(t))dt=v(t)∫tktk+1cos(θ(t))dt=vk∫tktk+1cos(θk+w(t−tk))dt=vkw[sin(wΔt+θ)−sin(θ)]{\int^{t_{k+1}}_{t_k} v(t)cos(\theta(t))dt} = {v(t) \int^{t_{k+1}}_{t_k} cos(\theta(t))dt} = {v_k \int^{t_{k+1}}_{t_k} cos(\theta_k + w (t-t_k))dt} = \frac {v_k} {w} {[sin(w \Delta t + \theta) - sin(\theta)]}$

$∫tktk+1v(t)sin(θ(t))dt=v(t)∫tktk+1sin(θ(t))dt=vk∫tktk+1sin(θk+w(t−tk))dt=vkw[−cos(wΔt+θ)+cos(θ)]{\int^{t_{k+1}}_{t_k} v(t)sin(\theta(t))dt}= {v(t) \int^{t_{k+1}}_{t_k} sin(\theta(t))dt} = {v_k \int^{t_{k+1}}_{t_k} sin(\theta_k + w (t-t_k))dt} = \frac {v_k} {w} {[-cos(w \Delta t + \theta) + cos(\theta)]}$

因此最终的状态方程为：

$Xk+1=Xk+[vkw[sin(wΔt+θ)−sin(θ)]vkw[−cos(wΔt+θ)+cos(θ)]0wΔt0]=[xk+vkw[sin(wΔt+θ)−sin(θ)]yk+vkw[−cos(wΔt+θ)+cos(θ)]vkwΔt+θw]X_{k+1} = X_k + \begin{bmatrix} {\frac {v_k} {w} {[sin(w \Delta t + \theta) - sin(\theta)]}}\\ {\frac {v_k} {w} {[-cos(w \Delta t + \theta) + cos(\theta)]}} \\ 0 \\ w \Delta t \\ 0 \end{bmatrix} = \begin{bmatrix} {x_k + \frac {v_k} {w} {[sin(w \Delta t + \theta) - sin(\theta)]}}\\ {y_k + \frac {v_k} {w} {[-cos(w \Delta t + \theta) + cos(\theta)]}} \\ v_k \\ w \Delta t + \theta \\ w \end{bmatrix}$

对状态方程的的每个状态量( $x,y,v,θ,wx,y,v,\theta ,w$ )求偏导得到雅各比矩阵 $J_F$ ：

$JF=[101w[sin(wΔt+θ)−sin(θ)]vkw[cos(wΔt+θ)−cos(θ)]vkΔtwcos(wΔt+θ)−vkw2[sin(wΔt+θ)−sin(θ)]011w[−cos(wΔt+θ)+cos(θ)]vkw[sin(wΔt+θ)−sin(θ)]vkΔtwsin(wΔt+θ)−vkw2[−cos(wΔt+θ)+cos(θ)]001000001Δt00001]J_F = \begin{bmatrix} 1 & 0 & {\frac 1{w} {[sin(w \Delta t + \theta) - sin(\theta)]}} & {\frac {v_k}{w} {[cos(w \Delta t + \theta) - cos(\theta)]}} & {\frac {v_k \Delta t}{w} cos(w \Delta t + \theta) - \frac {v_k} {w^2} {[sin(w \Delta t + \theta) - sin(\theta)]}} \\ 0 & 1 & {\frac 1{w} {[-cos(w \Delta t + \theta) + cos(\theta)]}} & {\frac {v_k}{w} {[sin(w \Delta t + \theta) - sin(\theta)]}} & {\frac {v_k \Delta t}{w} sin(w \Delta t + \theta) - \frac {v_k} {w^2} {[-cos(w \Delta t + \theta) + cos(\theta)]}} \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix}$

最低0.47元/天解锁文章