CV & CA & CTRV & CTRA

0. 运动模型简介
1. CV模型
2. CA模型
3. CTRV模型
4. CTRA模型

上一篇文章主要讲解了不同卡尔曼滤波的原理和特点，其中提到状态预测过程和状态更新两个主要的过程。在将卡尔曼滤波应用在车辆状态跟踪的问题中时，状态预测过程其实就是根据不同的运动模型来对车辆目标的状态进行预测。不同的运动模型是对实际车辆目标的运动过程进行一定的简化来建构的，其中包括一次运动模型和更高级的二次运动模型。本篇文章就是选用不同运动模型来分别构建卡尔曼滤波的模型（包括状态转移矩阵，过程噪声模型的构建和推导，以及不同模型对应的各个矩阵维度的分析）。

0. 运动模型简介

首先要明确的一点是，不管是什么运动模型，本质上都是为了帮助我们简化问题，所以我们可以根据运动模型的复杂程度来进行分类。

一次运动模型（也别称为线性运动模型）：

恒定速度模型（Constant Velocity, CV）
恒定加速度模型（Constant Acceleration, CA）

二次运动模型：

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

CTRV目前多用于机载追踪系统（飞机），这些二次运动模型大多假定速度vvv和偏航角速度（yaw rate） ω\omegaω没有关系，因此，在这类运动模型中，由于偏航角速度测量的扰动（不稳定），即使车辆没有移动，我们的运动模型下的角速度也会发生细微的变化。

为了解决这个问题，速度vvv和偏航角速度 ω\omegaω 的关联可以通过设定转向角Φ\PhiΦ恒定来建立，这样就引出了恒定转向角和速度模型（Constant Steering Angle and Velocity，CSAV）；另外，速度可以别假定为线性变化的，进而引出了常曲率和加速度模型（Constant Curvature and Acceleration，CCA）。

这些运动模型的关系如图：

本篇文章暂时只针对CV, CA, CTRV, CTRA四种运动模型进行建模和推导分析。

1. CV模型

恒定速度模型，（CV，Constant Velocity），模型的速度不变。恒定速度模型的状态空间：
x=(x,y,vx,vy)Tx=(x,y,v_x,v_y)^Tx=(x,y,vx,vy)T
依次为xxx坐标，yyy坐标，xxx速度，yyy速度。

对于x=(x,y,vx,vy)Tx=(x,y,v_x,v_y)^Tx=(x,y,vx,vy)T，推导运动模型以及状态转移矩阵。
xk+1=Akxk+Buk+ωkzk+1=Hxk+1+υk+1x_{k+1}=A_kx_{k}+Bu_k+\omega_k\\ z_{k+1}=Hx_{k+1}+\upsilon_{k+1}xk+1=Akxk+Buk+ωkzk+1=Hxk+1+υk+1

AAA是状态转移矩阵，BBB是可选的控制输入uku_kuk的增益，在大多数实际情况下并没有控制增益，所以BukBu_kBuk这一项为零； ωk\omega_kωk代表了过程噪声，它对应了每个分量的噪声，是期望为0，协方差矩阵为QQQ的高斯白噪声。

观测方程中, zk+1z_{k+1}zk+1是mmm阶向量，状态变量是nnn阶向量。HHH是m∗nm*nm∗n阶矩阵，代表状态变量xk+1x_{k+1}xk+1对测量变量zk+1z_{k+1}zk+1的增益。观测噪声υk+1\upsilon_{k+1}υk+1是期望为0，协方差矩阵为RRR的高斯白噪声。

已知上一时刻的状态，推导下一时刻的状态，因为是匀速模型速度不变，故可以根据物理原理s=s0+vΔts=s_0+v\Delta ts=s0+vΔt建模：
xk+1={xyvxvy}k+1={10Δt0010Δt00100001}{xyvxvy}kx_{k+1}=\left\{\begin{matrix}x \\ y \\v_x \\v_y\end{matrix}\right\}_{k+1}=\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\}_{k}xk+1=⎩⎪⎪⎨⎪⎪⎧xyvxvy⎭⎪⎪⎬⎪⎪⎫k+1=⎩⎪⎪⎨⎪⎪⎧10000100Δt0100Δt01⎭⎪⎪⎬⎪⎪⎫⎩⎪⎪⎨⎪⎪⎧xyvxvy⎭⎪⎪⎬⎪⎪⎫k

上式中的4*4的矩阵即为状态转移矩阵AAA。很显然，车辆不一定会以恒定的速度运动，所以过程包含了一定的过程噪声。在车辆的状态估计中的过程噪声其实就是车的加速度。由分析可知，影响速度的两个参数量，axa_xax和aya_yay。推导出加速度与位移的关系式s=12aΔt2s=\frac{1}{2}a{\Delta t}^2s=21aΔt2，即可得到过程噪声ω\omegaω。

ω={12axΔt212ayΔt2axΔtayΔt}={12Δt20012Δt2Δt00Δt}{axay}=Gu\omega=\left\{\begin{matrix}\frac{1}{2}a_x{\Delta t}^2 \\\frac{1}{2}a_y{\Delta t}^2 \\a_x\Delta t\\a_y\Delta t\end{matrix}\right\}=\left\{\begin{matrix}\frac{1}{2}{\Delta t}^2 & 0 \\0 & \frac{1}{2}{\Delta t}^2 \\\Delta t & 0\\0 & \Delta t\end{matrix}\right\} \left\{\begin{matrix}a_x\\a_y\end{matrix}\right\}=Guω=⎩⎪⎪⎨⎪⎪⎧21axΔt221ayΔt2axΔtayΔt⎭⎪⎪⎬⎪⎪⎫=⎩⎪⎪⎨⎪⎪⎧21Δt20Δt0021Δt20Δt⎭⎪⎪⎬⎪⎪⎫{axay}=Gu

u={axay}G={Δt2200Δt22Δt00Δt}u=\left\{\begin{matrix}a_x\\a_y\end{matrix}\right\} \quad G=\left\{\begin{matrix}\frac{\Delta t^2}{2} & 0\\0 & \frac{\Delta t^2}{2}\\\Delta t & 0\\0 & \Delta t \end{matrix}\right\}u={axay}G=⎩⎪⎪⎨⎪⎪⎧2Δt20Δt002Δt20Δt⎭⎪⎪⎬⎪⎪⎫

针对QQQ矩阵，通过最高阶状态量对其他量的扰动来初始化。
关于QQQ矩阵的公式推导：

Q=cov(ω)=E(ωωT)=GE(uuT)GT=G{σax200σay2}GTQ=cov(\omega)=E(\omega\omega^T)=GE(uu^T)G^T=G\left\{\begin{matrix}\sigma_{a_x}^2 & 0\\0 & \sigma_{a_y}^2\end{matrix}\right\}G^TQ=cov(ω)=E(ωωT)=GE(uuT)GT=G{σax200σay2}GT

Q={0.25Δt4σax200.5Δt3σax2000.25Δt4σay200.5Δt3σay20.5Δt3σax20Δt2σax2000.5Δt3σay20Δt2σay2}Q=\left\{\begin{matrix}0.25{\Delta t}^4{\sigma_{a_x}}^2 & 0 & 0.5{\Delta t}^3{\sigma_{a_x}}^2 & 0\\0 & 0.25{\Delta t}^4{\sigma_{a_y}}^2 & 0 &0.5{\Delta t}^3{\sigma_{a_y}}^2\\0.5{\Delta t}^3{\sigma_{a_x}}^2 & 0 & {\Delta t}^2{\sigma_{a_x}}^2 & 0\\0 & 0.5{\Delta t}^3{\sigma_{a_y}}^2 & 0 & {\Delta t}^2{\sigma_{a_y}}^2\end{matrix}\right\}Q=⎩⎪⎪⎨⎪⎪⎧0.25Δt4σax200.5Δt3σax2000.25Δt4σay200.5Δt3σay20.5Δt3σax20Δt2σax2000.5Δt3σay20Δt2σay2⎭⎪⎪⎬⎪⎪⎫

针对RRR矩阵，根据Gaussian的假设，取各个观测值最大偏差的三倍。尽管这在许多应用中并不合适（比如某些实验不可重复），但有一个准则还是好于任意值。本文中，针对CV模型中RRR矩阵设置如下：
R={σx200σy2}R=\left\{\begin{matrix}{\sigma_x}^2 & 0\\ 0 & {\sigma_y}^2\end{matrix}\right\}R={σx200σy2}

针对观测矩阵HHH，我们结合实际情况，要观测的是位置信息，所以H只需要取状态变量的前两个元素就够了，所以HHH设计成如下形式（注意HHH矩阵的维度）：
H={10000100}H=\left\{\begin{matrix}1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\end{matrix}\right\}H={10010000}

针对PPP矩阵，表示协方差矩阵，这个协方差矩阵只要确定了一开始的P0P_0P0, 后面可以递推出来，而且初始的协方差矩阵只要不是为0，它的取值对滤波效果影响很小，都能很快收敛。
P0={∑xx∑yy∑vxvx∑vyvy}P_0=\left\{\begin{matrix}\sum xx & & & \\ & \sum yy & & \\ & & \sum v_x v_x & \\ & & & \sum v_y v_y\end{matrix}\right\}P0=⎩⎪⎪⎨⎪⎪⎧∑xx∑yy∑vxvx∑vyvy⎭⎪⎪⎬⎪⎪⎫

总结：

A={10Δt0010Δt00100001}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⎭⎪⎪⎬⎪⎪⎫

B=0B=0B=0

Q={0.25Δt4σx200.5Δt3σx2000.25Δt4σy200.5Δt3σy20.5Δt3σx20Δt2σx2000.5Δt3σy20Δt2σy2}Q=\left\{\begin{matrix}0.25{\Delta t}^4{\sigma_x}^2 & 0 & 0.5{\Delta t}^3{\sigma_x}^2 & 0\\0 & 0.25{\Delta t}^4{\sigma_y}^2 & 0 &0.5{\Delta t}^3{\sigma_y}^2\\0.5{\Delta t}^3{\sigma_x}^2 & 0 & {\Delta t}^2{\sigma_x}^2 & 0\\0 & 0.5{\Delta t}^3{\sigma_y}^2 & 0 & {\Delta t}^2{\sigma_y}^2\end{matrix}\right\}Q=⎩⎪⎪⎨⎪⎪⎧0.25Δt4σx200.5Δt3σx2000.25Δt4σy200.5Δt3σy20.5Δt3σx20Δt2σx2000.5Δt3σy20Δt2σy2⎭⎪⎪⎬⎪⎪⎫

H={10000100}H=\left\{\begin{matrix}1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\end{matrix}\right\}H={10010000}

R={σx200σy2}R=\left\{\begin{matrix}{\sigma_x}^2 & 0\\ 0 & {\sigma_y}^2\end{matrix}\right\}R={σx200σy2}

P={∑xx∑yy∑vxvx∑vyvy}P=\left\{\begin{matrix}\sum xx & & & \\ & \sum yy & & \\ & & \sum v_x v_x & \\ & & & \sum v_y v_y\end{matrix}\right\}P=⎩⎪⎪⎨⎪⎪⎧∑xx∑yy∑vxvx∑vyvy⎭⎪⎪⎬⎪⎪⎫

已知状态空间的维度是4x1。

矩阵	A	B	Q	H	R	P
维度	4x4（与状态空间维度有关）	4×n（n为输入的列数）	4x4	2x4（2是z的维度，4是x的维度）	2x2（与z的维度有关）	4x4

CV模型的局限性：假设速度是常量，简化了车辆实际移动的形式，相对于匀加速模型来说过于简单。

2. CA模型

恒定加速度模型，（CA，constant acceleration），恒定加速度不变。CA模型有两种状态空间表达：
x=(x,y,vx,vy,ax,ay)Tx=(x,y,v_x,v_y,a_x,a_y)^Tx=(x,y,vx,vy,ax,ay)T
依次为xxx坐标，yyy坐标，xxx速度，yyy速度，xxx加速度，yyy加速度。

对于x=(x,y,vx,vy,ax,ay)Tx=(x,y,v_x,v_y,a_x,a_y)^Tx=(x,y,vx,vy,ax,ay)T，推导运动模型以及状态转移矩阵。

xk+1=Akxk+Buk+ωkzk+1=Hxk+1+υk+1x_{k+1}=A_kx_{k}+Bu_k+\omega_k\\ z_{k+1}=Hx_{k+1}+\upsilon_{k+1}xk+1=Akxk+Buk+ωkzk+1=Hxk+1+υk+1

已知上一时刻的状态，推导下一时刻的状态，因为是匀加速模型加速度不变，故可以根据物理原理s=s0+vΔt+12aΔt2s=s_0+v\Delta t+\frac{1}{2}a\Delta t^2s=s0+vΔt+21aΔt2建模，得到：
xk+1={xyvxvyaxay}k+1={10Δt012Δt20010Δt012Δt20010Δt000010Δt000010000001}{xyvxvyaxay}kx_{k+1}=\left\{\begin{matrix}x \\ y \\v_x \\v_y\\a_x\\a_y\end{matrix}\right\}_{k+1}= \left\{\begin{matrix}1 & 0 & \Delta t & 0 & \frac{1}{2}\Delta t^2 & 0\\0 & 1 & 0 & \Delta t & 0 &\frac{1}{2}\Delta t^2\\0 & 0 & 1 & 0 & \Delta t & 0\\0 & 0 & 0 &1 & 0 & \Delta t\\0 &0&0&0&1&0\\0&0&0&0&0&1\end{matrix}\right\} \left\{\begin{matrix}x \\ y \\v_x \\v_y\\a_x\\a_y\end{matrix}\right\}_{k}xk+1=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧xyvxvyaxay⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫k+1=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧100000010000Δt010000Δt010021Δt20Δt010021Δt20Δt01⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧xyvxvyaxay⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫k

则AAA矩阵为：
A={10Δt012Δt20010Δt012Δt20010Δt000010Δt000010000001}A= \left\{\begin{matrix} 1 & 0 & \Delta t & 0 & \frac{1}{2}\Delta t^2 & 0 \\0 & 1 & 0 & \Delta t & 0 &\frac{1}{2}\Delta t^2 \\0 & 0 & 1 & 0 & \Delta t & 0 \\0 & 0 & 0 &1 & 0 &\Delta t \\0 &0&0&0&1&0 \\0&0&0&0&0&1\end{matrix}\right\}A=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧100000010000Δt010000Δt010021Δt20Δt010021Δt20Δt01⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫

在匀加速车辆模型的状态估计中的过程噪声是车的加速度的变化率。由分析可知，影响速度的两个参数量，a˙x\dot{a}_xa˙x和a˙y\dot{a}_ya˙y。推导出位移和加速度变化率的关系式，s=16a˙Δt3s=\frac{1}{6} \dot{a}\Delta t^3s=61a˙Δt3，既可以得到过程噪声ω\omegaω。
ω={16a˙xΔt316a˙yΔt312a˙xΔt212a˙yΔt2a˙xΔta˙yΔt}={16Δt30016Δt312Δt20012Δt2Δt00Δt}{a˙xa˙y}=Gu\omega= \left\{\begin{matrix} \frac{1}{6}\dot{a}_x{\Delta t}^3 \\ \frac{1}{6}\dot{a}_y{\Delta t}^3 \\ \frac{1}{2}\dot{a}_x\Delta t^2\\ \frac{1}{2}\dot{a}_y\Delta t^2\\ \dot{a}_x\Delta t \\ \dot{a}_y\Delta t \\ \end{matrix}\right\}= \left\{\begin{matrix}\frac{1}{6}{\Delta t}^3 & 0 \\ 0 & \frac{1}{6}{\Delta t}^3 \\ \frac{1}{2}\Delta t^2 & 0\\0 & \frac{1}{2}\Delta t^2\\ \Delta t & 0\\ 0 & \Delta t\end{matrix}\right\} \left\{\begin{matrix}\dot{a}_x\\\dot{a}_y\end{matrix}\right\}=Guω=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧61a˙xΔt361a˙yΔt321a˙xΔt221a˙yΔt2a˙xΔta˙yΔt⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧61Δt3021Δt20Δt0061Δt3021Δt20Δt⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫{a˙xa˙y}=Gu

G={16Δt30016Δt312Δt20012Δt2Δt00Δt}u={a˙xa˙y}G=\left\{\begin{matrix}\frac{1}{6}{\Delta t}^3 & 0 \\ 0 & \frac{1}{6}{\Delta t}^3 \\ \frac{1}{2}\Delta t^2 & 0\\0 & \frac{1}{2}\Delta t^2\\ \Delta t & 0\\ 0 & \Delta t\end{matrix}\right\} \quad u = \left\{\begin{matrix}\dot{a}_x\\\dot{a}_y\end{matrix}\right\}G=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧61Δt3021Δt20Δt0061Δt3021Δt20Δt⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫u={a˙xa˙y}

针对QQQ矩阵，通过最高阶状态量对其他量的扰动来初始化。关于QQQ矩阵的公式推导：
Q=cov(ω)=E[(Gu)(Gu)T]=GE[u2]GT=G{σa˙x200σa˙y2}GTQ=cov(\omega)=E[(Gu)(Gu)^T]=GE[u^2]G^T=G\left\{\begin{matrix}{\sigma_{\dot{a}_x}}^2 & 0\\0 & {\sigma_{\dot{a}_y}}^2\end{matrix}\right\}G^TQ=cov(ω)=E[(Gu)(Gu)T]=GE[u2]GT=G{σa˙x200σa˙y2}GT

Q={16Δt30016Δt312Δt20012Δt2Δt00Δt}{σa˙x200σa˙y2}{16Δt30016Δt312Δt20012Δt2Δt00Δt}TQ=\left\{\begin{matrix}\frac{1}{6}{\Delta t}^3 & 0 \\ 0 & \frac{1}{6}{\Delta t}^3 \\ \frac{1}{2}\Delta t^2 & 0\\0 & \frac{1}{2}\Delta t^2\\ \Delta t & 0\\ 0 & \Delta t\end{matrix}\right\} \left\{\begin{matrix}{\sigma_{\dot{a}_x}}^2 & 0\\0 & {\sigma_{\dot{a}_y}}^2\end{matrix}\right\} \left\{\begin{matrix}\frac{1}{6}{\Delta t}^3 & 0 \\ 0 & \frac{1}{6}{\Delta t}^3 \\ \frac{1}{2}\Delta t^2 & 0\\0 & \frac{1}{2}\Delta t^2\\ \Delta t & 0\\ 0 & \Delta t\end{matrix}\right\}^TQ=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧61Δt3021Δt20Δt0061Δt3021Δt20Δt⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫{σa˙x200σa˙y2}⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧61Δt3021Δt20Δt0061Δt3021Δt20Δt⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫T

这里的R矩阵设置为：
R={σx200σy2}R=\left\{\begin{matrix}{\sigma_x}^2 & 0\\ 0 & {\sigma_y}^2\end{matrix}\right\}R={σx200σy2}

针对观测矩阵HHH，我们结合实际情况，要观测的是位置信息，所以H只需要取状态变量的前两个元素就够了，所以HHH设计成如下形式（要注意HHH矩阵的维度）：
H={100000010000}H=\left\{\begin{matrix}1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 &0 & 0\end{matrix}\right\}H={100100000000}

P矩阵如下：
P={∑xx∑yy∑vxvx∑vyvy∑axax∑ayay}P=\left\{\begin{matrix} \sum xx \\ & \sum yy \\ & & \sum v_x v_x \\ & & & \sum v_y v_y \\ & & & & \sum a_xa_x \\ & & & & &\sum a_ya_y \end{matrix}\right\}P=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧∑xx∑yy∑vxvx∑vyvy∑axax∑ayay⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫

总结：
A={10Δt012Δt20010Δt012Δt20010Δt000010Δt000010000001}A= \left\{\begin{matrix} 1 & 0 & \Delta t & 0 & \frac{1}{2}\Delta t^2 & 0 \\0 & 1 & 0 & \Delta t & 0 &\frac{1}{2}\Delta t^2 \\0 & 0 & 1 & 0 & \Delta t & 0 \\0 & 0 & 0 &1 & 0 &\Delta t \\0 &0&0&0&1&0 \\0&0&0&0&0&1\end{matrix}\right\}A=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧100000010000Δt010000Δt010021Δt20Δt010021Δt20Δt01⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫

B=0B = 0B=0

R={σx200σy2}R=\left\{\begin{matrix}{\sigma_x}^2 & 0\\ 0 & {\sigma_y}^2\end{matrix}\right\}R={σx200σy2}

H={100000010000}H=\left\{\begin{matrix}1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 &0 & 0\end{matrix}\right\} H={100100000000}

P={∑xx∑yy∑vxvx∑vyvy∑axax∑ayay}P=\left\{\begin{matrix} \sum xx \\ & \sum yy \\ & & \sum v_x v_x \\ & & & \sum v_y v_y \\ & & & & \sum a_xa_x \\ & & & & &\sum a_ya_y \end{matrix}\right\}P=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧∑xx∑yy∑vxvx∑vyvy∑axax∑ayay⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫

已知状态空间的维度是6x1。

矩阵	A	B	Q	H	R	P
维度	6x6（与状态空间维度有关）	6×n（n为输入的列数）	6x6	2x6（2是z的维度，4是x的维度）	2x2（与z的维度有关）	6x6

CA模型：相比于CV模型来说，速度由匀速变成了匀加速，但是由于大多数车辆道路是有拐弯的，CA模型会无法正确预测拐弯车辆。

3. CTRV模型

恒定转弯率和速度模型，（CTRV，Constant Turn Rate and Velocity），此模型假设对象沿直线前进，同时还能以固定的转弯速率和恒定的速度大小移动。

状态空间如下：
x=(x,y,v,ψ,ψ˙)Tx=(x, y, v, \psi, \dot{\psi})^Tx=(x,y,v,ψ,ψ˙)T

上式中变量依次为：xxx坐标，yyy坐标，vvv速度，ψ\psiψ偏航角（与xxx夹角逆时针为正），ψ˙\dot{\psi}ψ˙角速度。

由运动方程分析可以得到系统的差分方程：

x˙=vx=vcos⁡(ψ)y˙=vy=vsin⁡(ψ)v˙=0ψ˙=ψ˙ψ¨=0⇒{x˙y˙v˙ψ˙ψ¨}={vcos⁡(ψ)vsin⁡(ψ)0ψ˙0}\begin{matrix} \dot{x} = v_x=v\cos(\psi)\\ \dot{y} = v_y=v\sin(\psi)\\ \dot{v} = 0\\ \dot{\psi} = \dot{\psi}\\ \ddot{\psi} = 0 \end{matrix} \Rightarrow \left\{\begin{matrix} \dot{x}\\ \dot{y}\\ \dot{v}\\ \dot{\psi}\\ \ddot{\psi} \end{matrix}\right\} =\left\{\begin{matrix} v\cos(\psi)\\ v\sin(\psi)\\ 0\\ \dot{\psi}\\ 0\end{matrix}\right\} x˙=vx=vcos(ψ)y˙=vy=vsin(ψ)v˙=0ψ˙=ψ˙ψ¨=0⇒⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧x˙y˙v˙ψ˙ψ¨⎭⎪⎪⎪⎪⎬⎪⎪⎪⎪⎫=⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧vcos(ψ)vsin(ψ)0ψ˙0⎭⎪⎪⎪⎪⎬⎪⎪⎪⎪⎫

假设离散的时间步骤kkk和持续的时间值tkt_ktk相关，离散的时间步骤k+1k+1k+1和持续的时间值tk+1t_{k+1}tk+1相关，tk+1t_{k+1}tk+1和tkt_ktk之间的时间差叫做Δt\Delta tΔt。

当ψ˙k≠0\dot{\psi}_k \not=0ψ˙k=0时，
xk+1=xk+{∫tktk+1v(t)cos⁡(ψ(t))dt∫tktk+1v(t)sin⁡(ψ(t))dt0ψ˙kΔt0}x_{k+1}=x_{k}+ \left\{\begin{matrix} \int_{t_k}^{t_{k+1}}v(t)\cos(\psi(t))dt\\ \int_{t_k}^{t_{k+1}}v(t)\sin(\psi(t))dt\\ 0\\ \dot{\psi}_k \Delta t\\ 0 \end{matrix}\right\}xk+1=xk+⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧∫tktk+1v(t)cos(ψ(t))dt∫tktk+1v(t)sin(ψ(t))dt0ψ˙kΔt0⎭⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎫

将ψ(t)=ψk+ψ˙k(t−tk)\psi(t)=\psi_k + \dot{\psi}_k(t-t_k)ψ(t)=ψk+ψ˙k(t−tk)带入上式中，同时计算积分，得到：
xk+1=xk+{vkψ˙k(sin⁡(ψk+ψ˙kΔt)−sin⁡(ψk))vkψ˙k(−cos⁡(ψk+ψ˙kΔt)+cos⁡(ψk))0ψ˙kΔt0}x_{k+1}=x_{k}+ \left\{\begin{matrix} \frac{v_k}{\dot{\psi}_k}(\sin(\psi_k +\dot{\psi}_k \Delta t)-\sin(\psi_k))\\ \frac{v_k}{\dot{\psi}_k}(-\cos(\psi_k +\dot{\psi}_k \Delta t)+\cos(\psi_k))\\ 0\\ \dot{\psi}_k \Delta t\\ 0 \end{matrix}\right\}xk+1=xk+⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧ψ˙kvk(sin(ψk+ψ˙kΔt)−sin(ψk))ψ˙kvk(−cos(ψk+ψ˙kΔt)+cos(ψk))0ψ˙kΔt0⎭⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎫

当ψ˙k=0\dot{\psi}_k=0ψ˙k=0时，实际上是直线行驶，
x=vkcos⁡(ψk)Δty=vksin⁡(ψk)Δtx=v_k\cos(\psi_k)\Delta t\\ y=v_k\sin(\psi_k)\Delta tx=vkcos(ψk)Δty=vksin(ψk)Δt

CTRV模型中，假定车的纵向速度和角速度为恒定值，忽略了速度变化，因此把加速度项的影响放到了误差里面。

噪声分为两个部分，加速度υa,k\upsilon _{a,k}υa,k和υψ¨,k\upsilon _{\ddot{\psi},k}υψ¨,k，

υk={υa,kυψ¨,k}\upsilon _k= \left\{\begin{matrix}\upsilon _{a,k}\\\upsilon _{\ddot{\psi},k} \end{matrix}\right\}υk={υa,kυψ¨,k}

其中：
υa,k∼N(0,σa2)\upsilon _{a,k} \sim N(0,\sigma_a^2)υa,k∼N(0,σa2)纵向加速度噪声，表示在纵向速度上的不确定性；
υψ¨,k∼N(0,σψ..2)\upsilon _{\ddot{\psi},k} \sim N(0,\sigma_{\mathop{\psi}\limits^{..}}^2)υψ¨,k∼N(0,σψ..2)角加速度噪声，表示偏航角上的不确定性。

假设误差项，偏航角角加速度和纵向加速度是常量，那么在时间差内产生的误差a,b,c,d,ea,b,c,d,ea,b,c,d,e可定义为：

a=12υa,kcos⁡(ψk)Δt2b=12υa,ksin⁡(ψk)Δt2c=Δtυa,kd=12υψ¨,kΔt2e=Δtυψ¨,ka=\frac{1}{2}\upsilon _{a,k}\cos(\psi_k)\Delta t^2\\ b=\frac{1}{2}\upsilon _{a,k}\sin(\psi_k)\Delta t^2\\ c=\Delta t \upsilon _{a,k}\\ d=\frac{1}{2}\upsilon _{\ddot{\psi},k}\Delta t^2\\ e=\Delta t \upsilon _{\ddot{\psi},k} a=21υa,kcos(ψk)Δt2b=21υa,ksin(ψk)Δt2c=Δtυa,kd=21υψ¨,kΔt2e=Δtυψ¨,k

则完整的状态方程为：
{xk+1yk+1vk+1ψk+1ψ˙k+1}={xk+vkψk˙(sin⁡(ψk+ψk˙Δt)−sin⁡(ψk))yk+vkψk˙(−cos⁡(ψk+ψk˙Δt)+cos⁡(ψk))vkψk+ψk˙Δtψk˙}+{12υa,kcos⁡(ψk)Δt212υa,ksin⁡(ψk)Δt2Δtυa,k12υψ¨,kΔt2Δtυψ¨,k},ψk˙≠0\left\{\begin{matrix}x_{k+1}\\y_{k+1}\\v_{k+1}\\\psi_{k+1}\\\dot{\psi}_{k+1}\end{matrix}\right\}= \left\{\begin{matrix} x_k+\frac{v_k}{\dot{\psi_k}}(\sin(\psi_k +\dot{\psi_k} \Delta t)-\sin(\psi_k))\\ y_k+\frac{v_k}{\dot{\psi_k} }(-\cos(\psi_k +\dot{\psi_k} \Delta t)+\cos(\psi_k))\\ v_k\\ \psi_k+\dot{\psi_k} \Delta t\\ \dot{\psi_k} \end{matrix}\right\} + \left\{\begin{matrix} \frac{1}{2}\upsilon _{a,k}\cos(\psi_k)\Delta t^2\\ \frac{1}{2}\upsilon _{a,k}\sin(\psi_k)\Delta t^2\\ \Delta t \upsilon _{a,k}\\ \frac{1}{2}\upsilon _{\ddot{\psi},k}\Delta t^2\\ \Delta t \upsilon _{\ddot{\psi},k} \end{matrix}\right\}, \quad \dot{\psi_k}\neq0⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧xk+1yk+1vk+1ψk+1ψ˙k+1⎭⎪⎪⎪⎪⎬⎪⎪⎪⎪⎫=⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧xk+ψk˙vk(sin(ψk+ψk˙Δt)−sin(ψk))yk+ψk˙vk(−cos(ψk+ψk˙Δt)+cos(ψk))vkψk+ψk˙Δtψk˙⎭⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎫+⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧21υa,kcos(ψk)Δt221υa,ksin(ψk)Δt2Δtυa,k21υψ¨,kΔt2Δtυψ¨,k⎭⎪⎪⎪⎪⎬⎪⎪⎪⎪⎫,ψk˙=0

{xk+1yk+1vk+1ψk+1ψ˙k+1}={xk+vkcos⁡(ψk)Δtyk+vksin⁡(ψk)Δtvkψk+ψk˙Δtψk˙}+{12υa,kcos⁡(ψk)Δt212υa,ksin⁡(ψk)Δt2Δtυa,k12υψ¨,kΔt2Δtυψ¨,k},ψk˙=0\left\{\begin{matrix}x_{k+1}\\y_{k+1}\\v_{k+1}\\\psi_{k+1}\\\dot{\psi}_{k+1}\end{matrix}\right\}= \left\{\begin{matrix} x_k+v_k\cos(\psi_k)\Delta t\\ y_k+v_k\sin(\psi_k)\Delta t\\ v_k\\ \psi_k+\dot{\psi_k} \Delta t\\ \dot{\psi_k} \end{matrix}\right\} + \left\{\begin{matrix} \frac{1}{2}\upsilon _{a,k}\cos(\psi_k)\Delta t^2\\ \frac{1}{2}\upsilon _{a,k}\sin(\psi_k)\Delta t^2\\ \Delta t \upsilon _{a,k}\\ \frac{1}{2}\upsilon _{\ddot{\psi},k}\Delta t^2\\ \Delta t \upsilon _{\ddot{\psi},k} \end{matrix}\right\}, \quad \dot{\psi_k}=0⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧xk+1yk+1vk+1ψk+1ψ˙k+1⎭⎪⎪⎪⎪⎬⎪⎪⎪⎪⎫=⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧xk+vkcos(ψk)Δtyk+vksin(ψk)Δtvkψk+ψk˙Δtψk˙⎭⎪⎪⎪⎪⎬⎪⎪⎪⎪⎫+⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧21υa,kcos(ψk)Δt221υa,ksin(ψk)Δt2Δtυa,k21υψ¨,kΔt2Δtυψ¨,k⎭⎪⎪⎪⎪⎬⎪⎪⎪⎪⎫,ψk˙=0

可知CTRV模型是非线性的，需要线性化处理才能应用卡尔曼滤波。

过程噪声υ\upsilonυ
υ={12akcos⁡(ψk)Δt212aksin⁡(ψk)Δt2Δtak12ψ¨kΔt2Δtψk¨}={12cos⁡(ψk)Δt2012sin⁡(ψk)Δt20Δt0012Δt20Δt}{akψk¨}=Gu\upsilon= \left\{\begin{matrix} \frac{1}{2}a_k\cos(\psi_k)\Delta t^2\\ \frac{1}{2}a_k\sin(\psi_k)\Delta t^2\\ \Delta t a_k\\ \frac{1}{2}\ddot{\psi}_k\Delta t^2\\ \Delta t \ddot{\psi_k} \end{matrix}\right\}= \left\{\begin{matrix} \frac{1}{2}\cos(\psi_k)\Delta t^2 & 0\\ \frac{1}{2}\sin(\psi_k)\Delta t^2 & 0\\ \Delta t & 0\\ 0 & \frac{1}{2}\Delta t^2\\ 0 & \Delta t \end{matrix}\right\}\left\{\begin{matrix}a_k\\\ddot{\psi_k}\end{matrix}\right\} =Gu υ=⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧21akcos(ψk)Δt221aksin(ψk)Δt2Δtak21ψ¨kΔt2Δtψk¨⎭⎪⎪⎪⎪⎬⎪⎪⎪⎪⎫=⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧21cos(ψk)Δt221sin(ψk)Δt2Δt0000021Δt2Δt⎭⎪⎪⎪⎪⎬⎪⎪⎪⎪⎫{akψk¨}=Gu

针对QQQ矩阵，

Q=cov(υ)=E[(Gu)(Gu)T]=GE[u2]GT=G{σak200σψ¨k2}GTQ=cov(\upsilon)=E[(Gu)(Gu)^T]=GE[u^2]G^T=G\left\{\begin{matrix}{\sigma_{a_k}}^2 & 0\\0 & {\sigma_{\ddot{\psi}_k}}^2\end{matrix}\right\}G^TQ=cov(υ)=E[(Gu)(Gu)T]=GE[u2]GT=G{σak200σψ¨k2}GT

Q={12cos⁡(ψk)Δt2012sin⁡(ψk)Δt20Δt0012Δt20Δt}{σak200σψ¨k2}{12cos⁡(ψk)Δt2012sin⁡(ψk)Δt20Δt0012Δt20Δt}TQ=\left\{\begin{matrix} \frac{1}{2}\cos(\psi_k)\Delta t^2 & 0\\ \frac{1}{2}\sin(\psi_k)\Delta t^2 & 0\\ \Delta t & 0\\ 0 & \frac{1}{2}\Delta t^2\\ 0 & \Delta t \end{matrix}\right\} \left\{\begin{matrix}{\sigma_{a_k}}^2 & 0\\0 & {\sigma_{\ddot{\psi}_k}}^2\end{matrix}\right\} \left\{\begin{matrix} \frac{1}{2}\cos(\psi_k)\Delta t^2 & 0\\ \frac{1}{2}\sin(\psi_k)\Delta t^2 & 0\\ \Delta t & 0\\ 0 & \frac{1}{2}\Delta t^2\\ 0 & \Delta t \end{matrix}\right\}^TQ=⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧21cos(ψk)Δt221sin(ψk)Δt2Δt0000021Δt2Δt⎭⎪⎪⎪⎪⎬⎪⎪⎪⎪⎫{σak200σψ¨k2}⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧21cos(ψk)Δt221sin(ψk)Δt2Δt0000021Δt2Δt⎭⎪⎪⎪⎪⎬⎪⎪⎪⎪⎫T

针对HHH矩阵，需要观测的量为x，yx，yx，y的坐标和偏航角，故HHH是3x5维的矩阵。
H={100000100000010}H=\left\{\begin{matrix}1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 &0 \\ 0 & 0 & 0 & 1 & 0\end{matrix}\right\}H=⎩⎨⎧100010000001000⎭⎬⎫

针对PPP矩阵，在CTRV模型中，状态的数量nnn除了要包含5个状态以外，还要包括处理噪声，因为这些处理噪声对模型也有着非线性的影响。在增加了处理噪声的影响以后，我们的不确定性矩阵P就变成了：
P={P′00Q}P= \left\{\begin{matrix} P\prime & 0\\0 & Q \end{matrix}\right\}P={P′00Q}

4. CTRA模型

恒定转弯率和加速度模型。（CTRA，Constant Turn Rate and Acceleration）。同CTRV相比，转速不变，说明在相同时间内转过的角度是一样的，径向速度由匀速变成了匀加速，那么对应的位移会变长。

状态空间：
x=(x,y,ψ,v,a,ω)Tx=(x,y,\psi,v,a,\omega)^Tx=(x,y,ψ,v,a,ω)T

在状态空间中，车辆的加速度和角速度是不会发生变化的。

系统的差分方程：
x.=vx=(vk+aΔt)cos⁡(ψk+ωΔt)y.=vy=(vk+aΔt)sin⁡(ψk+ωΔt)ψ.=ωv.=aa.=0ω.=0\mathop{x}\limits^. =v_x=(v_k+a\Delta t)\cos(\psi_k+\omega \Delta t)\\ \mathop{y}\limits^. =v_y=(v_k+a\Delta t)\sin(\psi_k+\omega \Delta t)\\ \mathop{\psi}\limits^. =\omega\\ \mathop{v}\limits^. =a\\ \mathop{a}\limits^. =0\\ \mathop{\omega}\limits^. =0\\ x.=vx=(vk+aΔt)cos(ψk+ωΔt)y.=vy=(vk+aΔt)sin(ψk+ωΔt)ψ.=ωv.=aa.=0ω.=0

对差分方程，我们在tkt_ktk和tk+1t_{k+1}tk+1上积分，得到Δt(Δt=tk+1−tk)\Delta t(\Delta t=t_{k+1}-t_{k})Δt(Δt=tk+1−tk)时间段内的增量。运动方程如下：
xk+1=xk+{gx(xk,Δt)gy(xk,Δt)ωΔtaΔt00}x_{k+1}=x_k+ \left\{\begin{matrix} g_x(x_k,\Delta t)\\ g_y(x_k,\Delta t)\\ \omega \Delta t\\ a\Delta t\\ 0\\ 0 \end{matrix}\right\}xk+1=xk+⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧gx(xk,Δt)gy(xk,Δt)ωΔtaΔt00⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫

其中：
gx=aω2(cos⁡(ψk+ωΔt)−cos⁡(ψk))+1ω((vk+aΔt)sin⁡(ψk+ωΔt)−vksin⁡(ψk))g_x=\frac{a}{\omega ^2}(\cos(\psi_k+\omega \Delta t)-\cos(\psi_k))+\frac{1}{\omega}((v_k+a\Delta t)\sin(\psi_k+\omega\Delta t)-v_k\sin(\psi_k))gx=ω2a(cos(ψk+ωΔt)−cos(ψk))+ω1((vk+aΔt)sin(ψk+ωΔt)−vksin(ψk))

gy=aω2(sin⁡(ψk+ωΔt)−sin⁡(ψk))−1ω((vk+aΔt)cos⁡(ψk+ωΔt)−vkcos⁡(ψk))g_y=\frac{a}{\omega ^2}(\sin(\psi_k+\omega \Delta t)-\sin(\psi_k))-\frac{1}{\omega}((v_k+a\Delta t)\cos(\psi_k+\omega\Delta t)-v_k\cos(\psi_k))gy=ω2a(sin(ψk+ωΔt)−sin(ψk))−ω1((vk+aΔt)cos(ψk+ωΔt)−vkcos(ψk))

gxg_xgx和gyg_ygy的推导过程类似，这里推导一下gxg_xgx。

从上文中，我们得到了xxx的差分方程，进行积分，中间用到了一次分部积分，整理可得到最终答案，详细步骤如下：
x.=vx=(vk+aΔt)cos⁡(ψk+ωΔt)\mathop{x}\limits^. =v_x=(v_k+a\Delta t)\cos(\psi_k+\omega \Delta t)x.=vx=(vk+aΔt)cos(ψk+ωΔt)

gx=∫tktk+1(vk+aΔt)cos⁡(ψk+ωΔt)dtg_x=\int_{t_k}^{t_{k+1}}(v_k+a\Delta t)\cos(\psi_k+\omega \Delta t) dtgx=∫tktk+1(vk+aΔt)cos(ψk+ωΔt)dt

将括号打开，并将Δt=t−tk\Delta t=t-t_{k}Δt=t−tk带入，得到两项积分相加：
gx=∫tktk+1vkcos⁡(ψk+ω(t−tk))dt+∫tktk+1a(t−tk)cos⁡(ψk+ω(t−tk))dtg_x=\int_{t_k}^{t_{k+1}}v_k\cos(\psi_k+\omega (t-t_{k})) dt+\int_{t_k}^{t_{k+1}}a(t-t_{k})\cos(\psi_k+\omega (t-t_{k})) dtgx=∫tktk+1vkcos(ψk+ω(t−tk))dt+∫tktk+1a(t−tk)cos(ψk+ω(t−tk))dt

针对第一个积分项，只有可以coscoscos里面含有变量ttt，故可直接对ttt积分得到结果：

gx1=vkω(sin⁡(ψk+ωΔt)−sin⁡(ψk))g_{x1}=\frac{v_k}{\omega}(\sin(\psi_k+\omega \Delta t)-\sin(\psi_k))gx1=ωvk(sin(ψk+ωΔt)−sin(ψk))

针对第二项，接着将括号打开，得到两个积分项：

gx2=∫tktk+1atcos⁡(ψk+ω(t−tk))dt−∫tktk+1atkcos⁡(ψk+ω(t−tk))dtg_{x2}=\int_{t_k}^{t_{k+1}}at\cos(\psi_k+\omega(t-t_k))dt-\int_{t_k}^{t_{k+1}}at_k\cos(\psi_k+\omega(t-t_k))dtgx2=∫tktk+1atcos(ψk+ω(t−tk))dt−∫tktk+1atkcos(ψk+ω(t−tk))dt

可以看出第二个积分项是可以直接得出结果的，第一个积分项用分部积分，将cos⁡(ψk+ω(t−tk))dt\cos(\psi_k+\omega(t-t_k))dtcos(ψk+ω(t−tk))dt变成1ωdsin⁡(ψk+ω(t−tk))\frac{1}{\omega}d\sin(\psi_k+\omega(t-t_k))ω1dsin(ψk+ω(t−tk))，得到结果：

gx2=atk+1ωsin⁡(ψk+ωΔt)−atkωsin⁡(ψk)+aω2cos⁡(ψk+ωΔt)−aω2cos⁡(ψk)−atkω(sin⁡(ψk+ωΔt)−sin⁡(ψk))g_{x2}=\frac{at_{k+1}}{\omega}\sin(\psi_k+\omega \Delta t)-\frac{at_k}{\omega}\sin(\psi_k)+\frac{a}{\omega^2}\cos(\psi_k+\omega \Delta t)-\frac{a}{\omega^2}\cos(\psi_k)-\frac{at_k}{\omega}(\sin(\psi_k+\omega \Delta t)-\sin(\psi_k))gx2=ωatk+1sin(ψk+ωΔt)−ωatksin(ψk)+ω2acos(ψk+ωΔt)−ω2acos(ψk)−ωatk(sin(ψk+ωΔt)−sin(ψk))

将两个积分大项合并，得到最后的结果：

gx=aω2(cos⁡(ψk+ωΔt)−cos⁡(ψk))+1ω((vk+aΔt)sin⁡(ψk+ωΔt)−vksin⁡(ψk))g_x=\frac{a}{\omega ^2}(\cos(\psi_k+\omega \Delta t)-\cos(\psi_k))+\frac{1}{\omega}((v_k+a\Delta t)\sin(\psi_k+\omega\Delta t)-v_k\sin(\psi_k))gx=ω2a(cos(ψk+ωΔt)−cos(ψk))+ω1((vk+aΔt)sin(ψk+ωΔt)−vksin(ψk))

CTRA模型保持加速度和角速度不变，过程噪声是加速度和角速度的高阶项υ\upsilonυ：

υ={16a.Δt3cos⁡(ψk+12ω.Δt2)16a.Δt3sin⁡(ψk+12ω.Δt2)12ω.Δt212a.Δt2a.Δtω.Δt}\upsilon=\left\{\begin{matrix} \frac{1}{6}\mathop{a}\limits^. \Delta t^3 \cos(\psi_k+\frac{1}{2}\mathop{\omega}\limits^. \Delta t^2)\\ \frac{1}{6}\mathop{a}\limits^. \Delta t^3 \sin(\psi_k+\frac{1}{2}\mathop{\omega}\limits^. \Delta t^2)\\ \frac{1}{2}\mathop{\omega}\limits^.\Delta t^2\\ \frac{1}{2}\mathop{a}\limits^.\Delta t^2\\ \mathop{a}\limits^. \Delta t\\ \mathop{\omega}\limits^. \Delta t \end{matrix}\right\} υ=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧61a.Δt3cos(ψk+21ω.Δt2)61a.Δt3sin(ψk+21ω.Δt2)21ω.Δt221a.Δt2a.Δtω.Δt⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫

Q=cov(υ)Q=cov(\upsilon)Q=cov(υ)

整理得到最终的运动方程：

{xk+1yk+1ψk+1vk+1ak+1ωk+1}={xkykψkvkakωk}+{aω2(cos⁡(ψk+ωΔt)−cos⁡(ψk))+1ω((vk+aΔt)sin⁡(ψk+ωΔt)−vksin⁡(ψk))aω2(sin⁡(ψk+ωΔt)−sin⁡(ψk))−1ω((vk+aΔt)cos⁡(ψk+ωΔt)−vkcos⁡(ψk))ωΔtaΔt00}+{16a.Δt3cos⁡(ψk+12ω.Δt2)16a.Δt3sin⁡(ψk+12ω.Δt2)12ω.Δt212a.Δt2a.Δtω.Δt}\left\{\begin{matrix}x_{k+1}\\y_{k+1}\\\psi_{k+1}\\v_{k+1}\\a_{k+1}\\\omega_{k+1}\end{matrix}\right\}= \left\{\begin{matrix}x_{k}\\y_{k}\\\psi_{k}\\v_{k}\\a_{k}\\\omega_{k}\end{matrix}\right\} + \left\{\begin{matrix} \frac{a}{\omega ^2}(\cos(\psi_k+\omega \Delta t)-\cos(\psi_k))+\frac{1}{\omega}((v_k+a\Delta t)\sin(\psi_k+\omega\Delta t)-v_k\sin(\psi_k))\\ \frac{a}{\omega ^2}(\sin(\psi_k+\omega \Delta t)-\sin(\psi_k))-\frac{1}{\omega}((v_k+a\Delta t)\cos(\psi_k+\omega\Delta t)-v_k\cos(\psi_k))\\ \omega \Delta t\\ a\Delta t\\ 0\\ 0 \end{matrix}\right\} \\ \quad + \left\{\begin{matrix} \frac{1}{6}\mathop{a}\limits^. \Delta t^3 \cos(\psi_k+\frac{1}{2}\mathop{\omega}\limits^. \Delta t^2)\\ \frac{1}{6}\mathop{a}\limits^. \Delta t^3 \sin(\psi_k+\frac{1}{2}\mathop{\omega}\limits^. \Delta t^2)\\ \frac{1}{2}\mathop{\omega}\limits^.\Delta t^2\\ \frac{1}{2}\mathop{a}\limits^.\Delta t^2\\ \mathop{a}\limits^. \Delta t\\ \mathop{\omega}\limits^. \Delta t \end{matrix}\right\} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧xk+1yk+1ψk+1vk+1ak+1ωk+1⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧xkykψkvkakωk⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫+⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧ω2a(cos(ψk+ωΔt)−cos(ψk))+ω1((vk+aΔt)sin(ψk+ωΔt)−vksin(ψk))ω2a(sin(ψk+ωΔt)−sin(ψk))−ω1((vk+aΔt)cos(ψk+ωΔt)−vkcos(ψk))ωΔtaΔt00⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫+⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧61a.Δt3cos(ψk+21ω.Δt2)61a.Δt3sin(ψk+21ω.Δt2)21ω.Δt221a.Δt2a.Δtω.Δt⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫

针对HHH矩阵，需要观测的量为x，yx，yx，y的坐标和偏航角，故HHH是3x6维的矩阵。
H={100000010000001000}H=\left\{\begin{matrix}1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 &0 & 0\\ 0 & 0 & 1 & 0 & 0 & 0\end{matrix}\right\}H=⎩⎨⎧100010001000000000⎭⎬⎫

自动驾驶算法-滤波器系列（三）——不同运动模型（CV、CA、CTRV、CTRA）的建模和推导相关推荐

自动驾驶算法-滤波器系列（五）——高级运动模型在UKF中的应用
CTRV_UKF, CTRA_UKF 1. 基于CTRV的UKF 1.1 CTRV模型 1.2 UKF模型 2. 基于CTRA的UKF 2.1 CTRA模型扩展卡尔曼滤波算法是对非线性的系统方程或者 ...
自动驾驶算法-滤波器系列（四）——不同运动模型在KF/EKF中的应用
不同运动模型应用于KF/EKF 1. 基于匀速(CV)运动模型的KF 2. 基于匀加速(CA)运动模型的KF 3. 基于CTRV的EKF 4. 基于CTRA的EKF 上一篇文章中,笔者针对四种不同的运 ...
自动驾驶算法-滤波器系列（八）——IMM交互多模型介绍
IMM交互多模型介绍 1. 简介 (1)IMM(Interacting Multiple Model) (2)马尔科夫概率转移矩阵 2. 算法流程 (1)输入交互(模型j) (2)卡尔曼滤波(模型j) ...
自动驾驶算法-滤波器系列（二）—— 卡尔曼滤波简介及其变种（EKF、UKF、PF）介绍
KF&EKF&UKF&PF 1. 基础知识概要协方差矩阵多维高斯分布状态空间表达式 2. 什么是卡尔曼滤波器 3. 五个重要的公式公式介绍公式推导过程 4. 卡尔曼滤 ...
自动驾驶算法-滤波器系列（七）——ESKF(error-state Kalman Filter)介绍
IMM(Interacting Multiple Model) 1. ESKF是什么? 2. ESKF如何演变出来的? 3. ESKF主要解决什么问题? 4. ESKF算法原理 5. 总结 1. ES ...
自动驾驶算法-滤波器系列（六）——10+种经典滤波算法
经典滤波器介绍 1.限幅滤波法(又称程序判断滤波法) 2.中位值滤波法 3.算术平均滤波法 4.递推平均滤波法(又称滑动平均滤波法) 5.中位值平均滤波法(又称防脉冲干扰平均滤波法) 6.限幅平均滤波 ...
自动驾驶算法-滤波器系列（一）——详解卡尔曼滤波原理
详解卡尔曼滤波原理什么是卡尔曼滤波? 我们能用卡尔曼滤波做什么? 卡尔曼滤波是如何看到你的问题的使用矩阵来描述问题外部控制量外部干扰用测量量来修正估计值融合高斯分布将所有公式整合起来总 ...
自动驾驶采标系列三：基于图像的目标检测技术
标注猿的第54篇原创一个用数据视角看AI世界的标注猿上一篇文章我们从"环境感知"数据的采集设备上进行了详细说明,已经了解了相应设备采集的数据及采集前 ...
手撕自动驾驶算法——IMU测量模型、运动模型、误差模型
目录 IMU测量模型 IMU运动模型旋转量求导科氏加速度 IMU 误差模型确定性误差确定性误差误差标定六面法标定加速度六面法标定陀螺仪温度相关的参数标定随机误差高斯白噪声与随机游走 ...

自动驾驶算法-滤波器系列（三）——不同运动模型（CV、CA、CTRV、CTRA）的建模和推导