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

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

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

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

1 扩展卡尔曼滤波EKF

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

预测

${\hat{x}}_{k|k-1}=f(x_{k-1},u_k,0)$

$P_{k|k-1}=J_F{P}_{k-1|k-1}{J}_F^T+Q_k$

使用Jacobians矩阵更新模型

$J_F=\frac{\partial f}{\partial x}{|}_{{\hat{x}}_{k-1|k-1},u_k}$

$J_H=\frac{\partial h}{\partial x}{|}_{{\hat{x}}_{k|k-1}}$

更新

${\hat{y}}_k=z_k-h({\hat{x}}_{k|k-1},0)$

$S_k=J_H{P}_{k|k-1}{H}_k^T+R_k$

$K_k=P_{k|k-1}{J}_H^T{S}_k^{-1}$

${\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)

状态量为：

$$X=\left[\begin{array}{c}x\\ y\\ v\\ \theta \\ \omega \end{array}\right]$$

状态方程为：

$$X_{k+1}=X_k+\left[\begin{array}{c}{\int }_{t_k}^{t_{k+1}}v(t)cos(\theta (t))\\ {\int }_{t_k}^{t_{k+1}}v(t)sin(\theta (t))\\ 0\\ w\Delta t\\ 0\end{array}\right]$$

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

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

$${\int }_{t_k}^{t_{k+1}}v(t)cos(\theta (t))dt=v(t){\int }_{t_k}^{t_{k+1}}cos(\theta (t))dt=v_k{\int }_{t_k}^{t_{k+1}}cos({\theta }_k+w(t-t_k))dt=\frac{v_k}{w}[sin(w\Delta t+\theta )-sin(\theta )]$$

$${\int }_{t_k}^{t_{k+1}}v(t)sin(\theta (t))dt=v(t){\int }_{t_k}^{t_{k+1}}sin(\theta (t))dt=v_k{\int }_{t_k}^{t_{k+1}}sin({\theta }_k+w(t-t_k))dt=\frac{v_k}{w}[-cos(w\Delta t+\theta )+cos(\theta )]$$

因此最终的状态方程为：

$$X_{k+1}=X_k+\left[\begin{array}{c}\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{array}\right]=\left[\begin{array}{c}{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{array}\right]$$

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

$$J_F=\left[\begin{array}{ccccc}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{array}\right]$$

对于观测矩阵，如果是线性的，不用做计算。如果是非线性的，同样可以使用求偏导数的方式计算出雅各比矩阵$J_H$。

假设传感器是毫米波雷达观，且观测量是:
$$z_k=\left[\begin{array}{c}{\rho }_k\\ {\theta }_k\\ {\dot{\rho }}_k\end{array}\right]=\left[\begin{array}{c}\sqrt{x_k^2+y_k^2}\\ atan2(y_k,x_k)\\ \frac{v_x{x}_k+v_y{y}_k}{\sqrt{x_k^2+y_k^2}}\end{array}\right]=\left[\begin{array}{c}\sqrt{x_k^2+y_k^2}\\ atan2(y_k,x_k)\\ \frac{x_k{v}_{k}cos\theta +y_k{v}_{k}sin\theta }{\sqrt{x_k^2+y_k^2}}\end{array}\right]$$

上式对状态方程的的每个状态量($x,y,v,\theta ,w$)求偏导得到雅各比矩阵，即观测矩阵$J_H$为：
$$J_H=\left[\begin{array}{ccccc}\frac{x_k}{\sqrt{x_k^2+y_k^2}}& \frac{y_k}{\sqrt{x_k^2+y_k^2}}& 0& 0& 0\\ -\frac{y_k}{x_k^2+y_k^2}& \frac{x_k}{x_k^2+y_k^2}& 0& 0& 0\\ \frac{y_k(v_{k}cos\theta y_k-v_{k}sin\theta x_k)}{(x_k^2+y_k^2{)}^{\frac{3}{2}}}& \frac{x_k(v_{k}sin\theta x_k-v_{k}cos\theta y_k)}{(x_k^2+y_k^2{)}^{\frac{3}{2}}}& \frac{(x_{k}cos\theta +y_{k}sin\theta )}{\sqrt{x_k^2+y_k^2}}& \frac{(v_{k}cos\theta y_k-v_{k}sin\theta x_k)}{\sqrt{x_k^2+y_k^2}}& 0\end{array}\right]$$

补充，另外提一下cv模型：

对于自动驾驶常用CV模型的状态量：
$$X=\left[\begin{array}{c}x\\ y\\ v_x\\ v_y\end{array}\right]$$
其状态转移矩阵是线性的就不用求雅各比矩阵了，如果观测是radar: $[\rho ,\theta ,\dot{\rho }]$的话，其对应的观测转移矩阵$J_H$为：
$$J_H=\left[\begin{array}{cccc}\frac{x_k}{\sqrt{x_k^2+y_k^2}}& \frac{y_k}{\sqrt{x_k^2+y_k^2}}& 0& 0\\ -\frac{y_k}{x_k^2+y_k^2}& \frac{x_k}{x_k^2+y_k^2}& 0& 0\\ \frac{y_k(v_x{y}_k-v_y{x}_k)}{(x_k^2+y_k^2{)}^{\frac{3}{2}}}& \frac{x_k(v_y{x}_k-v_x{y}_k)}{(x_k^2+y_k^2{)}^{\frac{3}{2}}}& \frac{x_k}{\sqrt{x_k^2+y_k^2}}& \frac{y_k}{\sqrt{x_k^2+y_k^2}}\end{array}\right]$$

3 恒定转弯率和加速度模型(Constant Turn Rate and Acceleration,CTRA)

公式量太大，用Python导出的公式直接粘贴了。

状态量为：
$$X_k=\left[\begin{array}{c}x\\ y\\ \theta \\ v\\ w\\ a\end{array}\right]$$

状态方程为：

$$X_{k+1}=X_k+\left[\begin{array}{c}{\int }_{t_k}^{t_{k+1}}v(t)cos(\theta (t))\\ {\int }_{t_k}^{t_{k+1}}v(t)sin(\theta (t))\\ w\Delta t\\ a\Delta t\\ 0\\ 0\end{array}\right]$$

其中$a$和$w$为常量,和时间无关，$\Delta t=t_{k+1}-t_k$，$\omega \ne 0$，积分化简为：

$x_k$项积分：

$${\int }_{t_k}^{t_{k+1}}v(t)cos(\theta (t))dt={\int }_{t_k}^{t_{k+1}}(v_k+a(t-t_k))cos({\theta }_k+w(t-t_k))dt$$
$$={\int }_{t_k}^{t_{k+1}}{v}_{k}cos({\theta }_k+w(t-t_k))dt+{\int }_{t_k}^{t_{k+1}}a(t-t_k)cos({\theta }_k+w(t-t_k))dt=\frac{v_k}{\omega }[sin(w\Delta t+\theta )-sin(\theta )]+{\int }_{t_k}^{t_{k+1}}a(t-t_k)cos({\theta }_k+w(t-t_k))dt$$

对${\int }_{t_k}^{t_{k+1}}a(t-t_k)cos({\theta }_k+w(t-t_k))dt$，令$t-t_k=x\in (0,\Delta t)$,则$dt=dx$

$${\int }_{t_k}^{t_{k+1}}a(t-t_k)cos({\theta }_k+w(t-t_k))dt=a{\int }_0^{\Delta t}xcos({\theta }_k+wx)dx=\frac{a}{w^2}[wxsin(wx+{\theta }_k)+cos(wx+{\theta }_k)]{|}_0^{\Delta t}=\frac{a}{w^2}[cos(w\Delta t+{\theta }_k)-cos({\theta }_k)]+a\Delta t\frac{sin(w\Delta t+{\theta }_k)}{w}$$

所以合并为：
$${\int }_{t_k}^{t_{k+1}}v(t)cos(\theta (t))dt=\frac{1}{w^2}[(v_{k}w+aw\Delta t)sin({\theta }_k+w\Delta t)+acos({\theta }_k+w\Delta t)-v_{k}wsin({\theta }_k)-acos({\theta }_k)]$$

$y_k$项积分同上：
$${\int }_{t_k}^{t_{k+1}}v(t)sin(\theta (t))dt={\int }_{t_k}^{t_{k+1}}(v_k+a(t-t_k))cos({\theta }_k+w(t-t_k))dt$$

因此最终的状态方程为($T=\Delta t$)：

$$X=\left[\begin{array}{c}x+\frac{-wv\sin \left(\theta \right)-a\cos \left(\theta \right)+a\cos \left(Tw+\theta \right)+\left(Twa+wv\right)\sin \left(Tw+\theta \right)}{w^2}\\ y+\frac{wv\cos \left(\theta \right)-a\sin \left(\theta \right)+a\sin \left(Tw+\theta \right)+\left(-Twa-wv\right)\cos \left(Tw+\theta \right)}{w^2}\\ Tw+\theta \\ Ta+v\\ w\\ a\end{array}\right]$$

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

$$J_F=\left[\begin{array}{cccccc}1& 0& \frac{-wv\cos \left(\theta \right)+a\sin \left(\theta \right)-a\sin \left(Tw+\theta \right)+\left(Twa+wv\right)\cos \left(Tw+\theta \right)}{w^2}& \frac{-w\sin \left(\theta \right)+w\sin \left(Tw+\theta \right)}{w^2}& \frac{-Ta\sin \left(Tw+\theta \right)+T\left(Twa+wv\right)\cos \left(Tw+\theta \right)-v\sin \left(\theta \right)+\left(Ta+v\right)\sin \left(Tw+\theta \right)}{w^2}-\frac{2\left(-wv\sin \left(\theta \right)-a\cos \left(\theta \right)+a\cos \left(Tw+\theta \right)+\left(Twa+wv\right)\sin \left(Tw+\theta \right)\right)}{w^3}& \frac{Tw\sin \left(Tw+\theta \right)-\cos \left(\theta \right)+\cos \left(Tw+\theta \right)}{w^2}\\ 0& 1& \frac{-wv\sin \left(\theta \right)-a\cos \left(\theta \right)+a\cos \left(Tw+\theta \right)-\left(-Twa-wv\right)\sin \left(Tw+\theta \right)}{w^2}& \frac{w\cos \left(\theta \right)-w\cos \left(Tw+\theta \right)}{w^2}& \frac{Ta\cos \left(Tw+\theta \right)-T\left(-Twa-wv\right)\sin \left(Tw+\theta \right)+v\cos \left(\theta \right)+\left(-Ta-v\right)\cos \left(Tw+\theta \right)}{w^2}-\frac{2\left(wv\cos \left(\theta \right)-a\sin \left(\theta \right)+a\sin \left(Tw+\theta \right)+\left(-Twa-wv\right)\cos \left(Tw+\theta \right)\right)}{w^3}& \frac{-Tw\cos \left(Tw+\theta \right)-\sin \left(\theta \right)+\sin \left(Tw+\theta \right)}{w^2}\\ 0& 0& 1& 0& T& 0\\ 0& 0& 0& 1& 0& T\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$$

这也太长了太复杂了，谁会用这玩意儿。尽管求导一次就行，之后代数进去就行，但是也超级麻烦呀。

4 关于Q矩阵的公式推导

关于Q矩阵，一般是利用$\Delta t$和模型的噪声误差来赋值。(我以前是直接手工粗暴调的，因为不同模型的噪声方差基本上也都是自己设定的，差别不大。但是尽量按公式来吧。)

4.1 CTRV模型

比如对于CTRV，假设的直线加速度$a_a$和偏航角加速度$a_w$为常数，但是实际过程中会有噪声，因此CTRV的模型噪声主要来自$a_a$和$a_w$。假设该模型噪声都符合高斯分布，因此$a_a\sim N(0,{\sigma }_a^2)$，$a_w\sim N(0,{\sigma }_w^2)$。

$a_a,a_w$这两个加速度量对状态量($x,y,v,\theta ,\omega$)的预测噪声为：

$$w=\left[\begin{array}{c}\frac{1}{2}\Delta t^{2}cos(\theta )a_a\\ \frac{1}{2}\Delta t^{2}sin(\theta )a_a\\ \Delta t{a}_a\\ \frac{1}{2}\Delta t^2{a}_{\omega }\\ \Delta t{a}_{\omega }\end{array}\right]=\left[\begin{array}{cc}\frac{1}{2}\Delta t^{2}cos(\theta )& 0\\ \frac{1}{2}\Delta t^{2}sin(\theta )& 0\\ \Delta t& 0\\ 0& \frac{1}{2}\Delta t^2\\ 0& \Delta t{a}_{\omega }\end{array}\right]\left[\begin{array}{c}{a}_a\\ a_w\end{array}\right]=Gu$$

$$Q=cov(w)=E(w{w}^T)=GE(u{u}^T)G^T=G\left[\begin{array}{cc}{\sigma }_a^2& 0\\ 0& {\sigma }_w^2\end{array}\right]G^T$$

$$Q=\left[\begin{array}{ccccc}0.25T^4{\sigma }_a^2{\cos }^2\left(\theta \right)& 0.25T^4{\sigma }_a^2\sin \left(\theta \right)\cos \left(\theta \right)& 0.5T^3{\sigma }_a^2\cos \left(\theta \right)& 0& 0\\ 0.25T^4{\sigma }_a^2\sin \left(\theta \right)\cos \left(\theta \right)& 0.25T^4{\sigma }_a^2{\sin }^2\left(\theta \right)& 0.5T^3{\sigma }_a^2\sin \left(\theta \right)& 0& 0\\ 0.5T^3{\sigma }_a^2\cos \left(\theta \right)& 0.5T^3{\sigma }_a^2\sin \left(\theta \right)& T^2{\sigma }_a^2& 0& 0\\ 0& 0& 0& 0.25T^4{\sigma }_w^2& 0.5T^3{\sigma }_w^2\\ 0& 0& 0& 0.5T^3{\sigma }_w^2& T^2{\sigma }_w^2\end{array}\right]$$

4.2 CV模型

对于CV模型的话，噪声就是直线加速度a，其分量$a_x$和$a_y$对状态量($x,y,v_x,v_y$)预测噪声为：

$$w=\left[\begin{array}{c}\frac{1}{2}\Delta t^2{a}_x\\ \frac{1}{2}\Delta t^2{a}_y\\ \Delta t{a}_x\\ \Delta t{a}_y\end{array}\right]=\left[\begin{array}{cc}\frac{1}{2}\Delta t^2& 0\\ 0& \frac{1}{2}\Delta t^2\\ \Delta t& 0\\ 0& \Delta t\end{array}\right]\left[\begin{array}{c}{a}_x\\ a_y\end{array}\right]=Gu$$

$$Q=cov(w)=E(w{w}^T)=GE(u{u}^T)G^T=G\left[\begin{array}{cc}{\sigma }_x^2& 0\\ 0& {\sigma }_y^2\end{array}\right]G^T=\left[\begin{array}{cccc}0.25T^4{\sigma }_x^2& 0& 0.5T^3{\sigma }_x^2& 0\\ 0& 0.25T^4{\sigma }_y^2& 0& 0.5T^3{\sigma }_y^2\\ 0.5T^3{\sigma }_x^2& 0& T^2{\sigma }_x^2& 0\\ 0& 0.5T^3{\sigma }_y^2& 0& T^2{\sigma }_y^2\end{array}\right]$$