利用卡尔曼滤波进行状态估计

本文主要介绍如何利用卡尔曼滤波进行状态估计，因为需要用到一些车辆的运动模型，所以首先简单介绍几种常见的模型

车辆运动模型

匀速模型（Constant Velocity, CV）

四个状态变量依次为横坐标，纵坐标，与x轴夹角（逆时针为正），线速度
匀速和匀加速模型的状态转移方程都很好理解，可以直接得到

匀加速模型（Constant Acceleration, CA）

状态变量多了一个加速度

恒定速度和转速模型（Constant Turn Rate and Velocity, CTRV）

恒定速度和转速可以看做一段圆弧运动


该模型利用简单的几何关系也不难得到，后面两个模型稍微复杂一些。另外，CTRV实际上是CV的一般形式，当$\omega =0$时（这里需要用到洛必达法则），就是CV的形式。

恒定加速度和转速模型（Constant Turn Rate and Acceleration, CTRA）

同CTRV相比，转速不变，说明在相同时间内转过的角度是一样的，径向由匀速变成匀加速，那么对应的，位移会变长（可以参见CCA部分橘色轨迹）


其中

这个模型怎么得到呢？首先$\theta$和$v$的增量我们很好理解，而前两项利用CTRV的结果结合微元法，假设从t时刻开始，经历一个很小的时间dt内我们可以将CTRA模型近似为CTRV模型（回忆一下一起物理中利用微元法推导匀加速运动的过程），那么，dt时间内x，y的增量可以表示为
$${\Delta }_x=\frac{v+at}{\omega }[\sin ({\theta }_k+\omega t+wdt)-\sin ({\theta }_k+\omega t)]$$
$${\Delta }_y=\frac{v+at}{\omega }[\cos ({\theta }_k+\omega t)-\cos ({\theta }_k+\omega t+wdt)]$$
化简，当dt为小量时，${\Delta }_x$可以近似为
$${\Delta }_x=\frac{v+at}{\omega }\cos ({\theta }_k+\omega t)wdt$$
对该式从0到t积分，利用分部积分即可得到结果。

恒定曲率半径和加速度模型（Constant Curvature and Acceleration, CCA）

最后一种模型是将CTRA中的转速一定改为曲率半径一定，由$r=v/\omega$可知，当曲率半径一定，又要做匀加速运动，那么角速度必然是时变的，因此同样时间内转过的角度必然会增大。CTRV（黑），CTRA（橘），CCA（红）的关系如下图所示

设曲率半径$c=1/r$，则状态变量如下

此时，弧长为

则对应的旋转角$b_{k}c$，知道角度了求解转移关系的方法和CTRV一样，最后结果为
$$x_{k+1}=x_k+\lfloor$$