不同运动模型应用于KF/EKF

1. 基于匀速(CV)运动模型的KF
2. 基于匀加速(CA)运动模型的KF
3. 基于CTRV的EKF
4. 基于CTRA的EKF

上一篇文章中，笔者针对四种不同的运动模型进行了建模和推导分析，本篇文章将不同的运动模型应用于卡尔曼滤波或者扩展卡尔曼滤波中。CV和CA模型属于线性模型，直接用线性KF（卡尔曼滤波）即可。CTRV和CTRA模型属于非线性模型，需要对其进行线性化，需要结合EKF（扩展卡尔曼滤波）建立模型。

1. 基于匀速(CV)运动模型的KF

KF模型如下：
xk=Axk−1+Buk−1+wk−1zk=Hxk+vkx_k=Ax_{k-1}+Bu_{k-1}+w_{k-1}\\ z_k =Hx_k+v_kxk=Axk−1+Buk−1+wk−1zk=Hxk+vk

CV模型是线性的，故可以直接使用线性卡尔曼滤波。CV模型如下：
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

可以确定相关矩阵：
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⎭⎪⎪⎬⎪⎪⎫

AAA矩阵中的未知参数Δt\Delta tΔt可以由采样时间间隔得到。

在系统中默认没有输入量，B=0B=0B=0。

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⎭⎪⎪⎬⎪⎪⎫

QQQ矩阵中xxx方向的加速度和yyy方向的加速度是无法得到的，需要人为设定一个值。

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

根据要观测量的维度和状态量的维度，确定H矩阵的形式。

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

RRR矩阵传感器的测量噪声，无法确定。

P0={1111}P_0=\left\{\begin{matrix}1 & & & \\ &1 & & \\ & & 1 & \\ & & & 1\end{matrix}\right\}P0=⎩⎪⎪⎨⎪⎪⎧1111⎭⎪⎪⎬⎪⎪⎫

针对P0P_0P0初始化的问题，经过测试，卡尔曼滤波的结果对初始化的值的大小不敏感，故本模型采用相同维度的单位矩阵来初始化。

上述中的QQQ和RRR矩阵是无法得到确切值的，只能通过调参的方法来确定一个大概的数值，下面将展示QQQ和RRR的具体调参过程。

下表为各个矩阵的维度：

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

ＫＫＫ值只与Q、RQ、RQ、R的比值有关系，而与Ｑ，ＲＱ，ＲＱ，Ｒ的绝对值没有关系。所以在不同算法中，Ｒ，ＱＲ，ＱＲ，Ｑ的取值根据反应不同的量纲，可以有很多变化，但他们的比值决定了滤波值应该更多来自系统模型的演化信息，还是来自于观察信号信息。

QQQ值为过程噪声，越小系统越容易收敛，我们对模型预测的值信任度越高；但是太小则容易发散，如果QQQ为零，那么我们只相信预测值；QQQ值越大我们对于预测的信任度就越低，而对测量值的信任度就变高；如果QQQ值无穷大，那么我们只信任测量值。

RRR值为测量噪声，太小太大都不一定合适。RRR太大，卡尔曼滤波响应会变慢，因为它对新测量的值的信任度降低；越小系统收敛越快，但过小则容易出现震荡；测试时可以保持传感器不动，记录一段时间内的输出数据，这个数据近似正态分布，按3σ3\sigma3σ原则，取正态分布的(3σ)2(3\sigma)^2(3σ)2作为R的初始化值。

测试时可以先将QQQ从小往大调整，将RRR从大往小调整；先固定一个值去调整另外一个值，看收敛速度与波形输出。
当QQQ大，RRR小时，滤波更相信观测值，即滤波结果曲线更接近观测值；
当QQQ小，RRR大时，滤波更相信预测值，即滤波结果曲线更接近预测值。

2. 基于匀加速(CA)运动模型的KF

KF模型如下：
xk=Axk−1+Buk−1+wk−1zk=Hxk+vkx_k=Ax_{k-1}+Bu_{k-1}+w_{k-1}\\ z_k =Hx_k+v_kxk=Axk−1+Buk−1+wk−1zk=Hxk+vk

CA模型同样是线性模型，模型如下：
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

可以确定A矩阵：
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⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫

AAA矩阵中的未知参数Δt\Delta tΔt为采样时间间隔。

在系统中默认没有输入量，B=0B=0B=0。
ω={16a˙xΔt316a˙yΔt312a˙xΔt212a˙Δ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}_\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˙Δ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}

上式中的ax⋅a_x^·ax⋅和ay⋅a_y^·ay⋅是需要我们人为确定的参数。
H={100000010000}H=\left\{\begin{matrix}1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 &0 & 0\end{matrix}\right\} H={100100000000}

根据要观测量的维度和状态量的维度，确定HHH矩阵的形式。
R={σx200σy2}R=\left\{\begin{matrix}{\sigma_x}^2 & 0\\ 0 & {\sigma_y}^2\end{matrix}\right\}R={σx200σy2}

RRR矩阵传感器的测量噪声，无法确定。
P0={111111}P_0=\left\{\begin{matrix} 1 \\ & 1 \\ & & 1 \\ & & & 1 \\ & & & & 1 \\ & & & & &1 \end{matrix}\right\}P0=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧111111⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫

P0P_0P0采用单位矩阵进行初始化。

QQQ和RRR的值需要经过不断地调参来确定。调参过程和CV_KF类似，此处就不在过多介绍。
下面是CA模型中矩阵的参数:
下表为各个矩阵的维度：

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

3. 基于CTRV的EKF

CTRV模型选取的状态空间为：
x=(x,y,v,ψ,ω)Tx=(x,y,v,\psi,\omega)^Tx=(x,y,v,ψ,ω)T
上式中变量依次为：xxx坐标，yyy坐标，速度，偏航角（与x夹角逆时针为正），角速度。

状态方程为：
{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}\\\omega_{k+1}\end{matrix}\right\}= \left\{\begin{matrix} x_k+\frac{v_k}{\omega_{k}}(\sin(\psi_k +\omega_{k} \Delta t)-\sin(\psi_k))\\ y_k+\frac{v_k}{\omega_{k}}(-\cos(\psi_k +\omega_{k} \Delta t)+\cos(\psi_k))\\ v_k\\ \psi_k+\omega_{k}\Delta t\\ \omega_{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 _{\mathop{\psi}\limits^{..},k}\Delta t^2\\ \Delta t \upsilon _{\mathop{\psi}\limits^{..},k} \end{matrix}\right\}, \quad \omega_{k}\neq0⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧xk+1yk+1vk+1ψk+1ωk+1⎭⎪⎪⎪⎪⎬⎪⎪⎪⎪⎫=⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧xk+ωkvk(sin(ψk+ωkΔt)−sin(ψk))yk+ωkvk(−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}\\\omega_{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+\omega_{k}\Delta t\\ \omega_{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 _{\mathop{\psi}\limits^{..},k}\Delta t^2\\ \Delta t \upsilon _{\mathop{\psi}\limits^{..},k} \end{matrix}\right\}, \quad \omega_{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
可以看出上面两个式子都为非线性的，采用雅各比矩阵对其进行线性化。相应的雅各比矩阵如下：

当ω≠0\omega \neq0ω=0时：
JA={101ω(−sin⁡(ψ)+sin⁡(Δtω+ψ))vω(−cos⁡(ψ)+cos⁡(Δtω+ψ))Δtvωcos⁡(Δtω+ψ)−vω2(−sin⁡(ψ)+sin⁡(Δtω+ψ))01vω(−sin⁡(ψ)+sin⁡(Δtω+ψ))1ω(cos⁡(ψ)−cos⁡(Δtω+ψ))Δtvωsin⁡(Δtω+ψ)−vω2(cos⁡(ψ)−cos⁡(Δtω+ψ))001000001Δt00001}J_A = \left\{\begin{matrix}1 & 0 & \frac{1}{\omega} (-\sin(\psi)+\sin(\Delta t\omega+\psi)) & \frac{v}{\omega}(-\cos(\psi)+\cos(\Delta t\omega+\psi)) & \frac{\Delta t v}{\omega}\cos(\Delta t\omega + \psi)-\frac{v}{\omega^2}(-\sin(\psi)+\sin(\Delta t \omega+\psi))\\ 0 & 1 & \frac{v}{\omega} (-\sin(\psi)+\sin(\Delta t\omega+\psi)) & \frac{1}{\omega}(\cos(\psi)-\cos(\Delta t\omega+\psi)) & \frac{\Delta t v}{\omega}\sin(\Delta t\omega + \psi)-\frac{v}{\omega^2}(\cos(\psi)-\cos(\Delta t \omega+\psi))\\ 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 1 & \Delta t\\ 0 & 0 & 0 & 0 & 1 \end{matrix}\right\}JA=⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧1000001000ω1(−sin(ψ)+sin(Δtω+ψ))ωv(−sin(ψ)+sin(Δtω+ψ))100ωv(−cos(ψ)+cos(Δtω+ψ))ω1(cos(ψ)−cos(Δtω+ψ))010ωΔtvcos(Δtω+ψ)−ω2v(−sin(ψ)+sin(Δtω+ψ))ωΔtvsin(Δtω+ψ)−ω2v(cos(ψ)−cos(Δtω+ψ))0Δt1⎭⎪⎪⎪⎪⎬⎪⎪⎪⎪⎫

当ω=0\omega = 0ω=0时：
JA={10Δtcos⁡(ψ)−Δtvsin⁡(ψ)001Δtsin⁡(ψ)Δtvcos⁡(ψ)0001000001Δt00001}J_A = \left\{\begin{matrix} 1 & 0 & \Delta t\cos(\psi) & -\Delta t v\sin(\psi) & 0\\ 0 & 1 & \Delta t\sin(\psi) & \Delta t v\cos(\psi) & 0\\ 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 1 & \Delta t\\ 0 & 0 & 0 & 0 & 1 \end{matrix}\right\}JA=⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧1000001000Δtcos(ψ)Δtsin(ψ)100−Δtvsin(ψ)Δtvcos(ψ)010000Δt1⎭⎪⎪⎪⎪⎬⎪⎪⎪⎪⎫

对于HHH矩阵，根据传感器采集到的数据（xxx坐标，yyy坐标，偏航角）以及状态空间的大小，确定HHH矩阵：

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⎭⎬⎫

QQQ矩阵的推导见不同运动模型的建模和推导，此处直接给出结论：
Q=cov(υ)=E[(Gu)(Gu)T]=GE[u2]GT=G{σak200σψk..2}GTQ=cov(\upsilon)=E[(Gu)(Gu)^T]=GE[u^2]G^T=G\left\{\begin{matrix}{\sigma_{a_k}}^2 & 0\\0 & {\sigma_{\mathop{\psi_k}\limits^{..}}}^2\end{matrix}\right\}G^TQ=cov(υ)=E[(Gu)(Gu)T]=GE[u2]GT=G{σak200σψk..2}GT

Q={12cos⁡(ψk)Δt2012sin⁡(ψk)Δt20Δt0012Δt20Δt}{σak200σψk..2}{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_{\mathop{\psi_k}\limits^{..}}}^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σψk..2}⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧21cos(ψk)Δt221sin(ψk)Δt2Δt0000021Δt2Δt⎭⎪⎪⎪⎪⎬⎪⎪⎪⎪⎫T

PPP矩阵的初始化选择单位矩阵I5∗5I_{5*5}I5∗5初始化。RRR矩阵为测量噪声矩阵，采用对角矩阵初始化，具体参数需要后面慢慢调。QQQ矩阵的噪声参数也需要根据后期调参来确定。

EKF的增益矩阵KKK和KF的不同点仅仅在于AAA的形式不同，KF中的ＡＡＡ矩阵为状态方程的状态转移矩阵，EKF中的AAA为线性化之后的矩阵，即JAJ_AJA。将线性中的AAA矩阵替换为JAJ_AJA，即可得到EKF的卡尔曼滤波公式。

下面是各个参数矩阵的维度：

矩阵	A	B	Q	H	R	P	X	I	K	G	J_A
维度	5x5（与状态空间维度有关）	5×n（n为输入的列数）	5x5	3x5（3是z的维度，5是x的维度）	3x3（与z的维度有关）	5x5	5x1	5x5	5x3	5x2	5x5

4. 基于CTRA的EKF

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

上式中变量依次为：xxx坐标，yyy坐标，偏航角，速度，加速度，角速度。

状态方程为：
当ω≠0\omega \neq0ω=0时：

{xk+1yk+1ψk+1vk+1ak+1ωk+1}={xk+aω2(cos⁡(ψk+ωΔt)−cos⁡(ψk))+1ω((vk+aΔt)sin⁡(ψk+ωΔt)−vksin⁡(ψk))yk+aω2(sin⁡(ψk+ωΔt)−sin⁡(ψk))−1ω((vk+aΔt)cos⁡(ψk+ωΔt)−vkcos⁡(ψk))ψk+ωΔtvk+aΔtakωk}\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} + \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))\\ y_{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))\\ \psi_{k} + \omega \Delta t\\ v_{k} + a\Delta t\\ a_{k}\\ \omega_{k} \end{matrix}\right\} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧xk+1yk+1ψk+1vk+1ak+1ωk+1⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧xk+ω2a(cos(ψk+ωΔt)−cos(ψk))+ω1((vk+aΔt)sin(ψk+ωΔt)−vksin(ψk))yk+ω2a(sin(ψk+ωΔt)−sin(ψk))−ω1((vk+aΔt)cos(ψk+ωΔt)−vkcos(ψk))ψk+ωΔtvk+aΔtakωk⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫

当ω=0\omega = 0ω=0时：

{xk+1yk+1ψk+1vk+1ak+1ωk+1}={xk+(vkΔt+12aΔt2)cos⁡(ψk)yk+(vkΔt+12aΔt2)sin⁡(ψk)ψkvk+aΔtak0}\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 + (v_k\Delta t+\frac{1}{2}a\Delta t^2)\cos(\psi_k)\\ y_k + (v_k\Delta t+\frac{1}{2}a\Delta t^2)\sin(\psi_k)\\ \psi_k\\ v_k + a\Delta t\\ a_k\\ 0 \end{matrix}\right\} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧xk+1yk+1ψk+1vk+1ak+1ωk+1⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧xk+(vkΔt+21aΔt2)cos(ψk)yk+(vkΔt+21aΔt2)sin(ψk)ψkvk+aΔtak0⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫

线性化，求出相应的雅各比矩阵：
ω≠0\omega \neq 0ω=0:
JA={10f11ωsin⁡(ψk+ωΔt)−sin⁡(ψk)Δtωsin⁡(ψk+ωΔt)f201f3−1ωcos⁡(ψk+ωΔt)−cos⁡(ψk)−Δtωcos⁡(ψk+ωΔt)f400100Δt0001Δt0000010000001}J_A = \left\{\begin{matrix} 1 & 0 & f_1 & \frac{1}{\omega}\sin(\psi_k+\omega \Delta t)-\sin(\psi_k) & \frac{\Delta t}{\omega}\sin(\psi_k+\omega \Delta t ) & f_2\\ 0 & 1 & f_3 & -\frac{1}{\omega}\cos(\psi_k+\omega \Delta t)-\cos(\psi_k) & -\frac{\Delta t}{\omega}\cos(\psi_k+\omega \Delta t) & f4\\ 0 & 0 & 1 & 0 & 0 & \Delta t\\ 0 & 0 & 0 & 1 & \Delta t & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}\right\}JA=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧100000010000f1f31000ω1sin(ψk+ωΔt)−sin(ψk)−ω1cos(ψk+ωΔt)−cos(ψk)0100ωΔtsin(ψk+ωΔt)−ωΔtcos(ψk+ωΔt)0Δt10f2f4Δt001⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫
其中：
f1=aω2(−sin⁡(ψk+ωΔt)+sin⁡(ψk))+1ω(vk+aΔt)cos⁡(ψk+ωΔt)−vkcos⁡(ψk)f_1 = \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)f1=ω2a(−sin(ψk+ωΔt)+sin(ψk))+ω1(vk+aΔt)cos(ψk+ωΔt)−vkcos(ψk)

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

f3=aω2(cos⁡(ψk+ωΔt)−cos⁡(ψk))+1ω(vk+aΔt)sin⁡(ψk+ωΔt)+vksin⁡(ψk)f_3 = \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)f3=ω2a(cos(ψk+ωΔt)−cos(ψk))+ω1(vk+aΔt)sin(ψk+ωΔt)+vksin(ψk)

f4=−2aω3(sin⁡(ψk+ωΔt)−sin⁡(ψk))−aω2Δtsin⁡(ψk+Δtω)+1ω(vk+aΔt)Δtsin⁡(ψk+ωΔt)+1ω((vk+aΔt)sin⁡(ψk+ωΔt)−vksin⁡(ψk))f_4=\frac{-2a}{\omega^3}(\sin(\psi_k+\omega \Delta t)-\sin(\psi_k))-\frac{a}{\omega^2}\Delta t\sin(\psi_k+\Delta t\omega) + \frac{1}{\omega}(v_k+a\Delta t)\Delta t \sin(\psi_k+\omega \Delta t)+\frac{1}{\omega}((v_k+a\Delta t)\sin(\psi_k+\omega \Delta t)-v_k\sin(\psi_k))f4=ω3−2a(sin(ψk+ωΔt)−sin(ψk))−ω2aΔtsin(ψk+Δtω)+ω1(vk+aΔt)Δtsin(ψk+ωΔt)+ω1((vk+aΔt)sin(ψk+ωΔt)−vksin(ψk))

ω=0\omega = 0ω=0:
JA={10−sin⁡(ψk)(vkΔt+12aΔt2)Δtcos⁡(ψk)12Δt2cos⁡(ψk)001(vkΔt+12aΔt2)cos⁡(ψk)Δtsin⁡(ψk)12Δt2sin⁡(ψk)00010000001Δt0000010000000}J_A = \left\{\begin{matrix} 1 & 0 & -\sin(\psi_k)(v_k\Delta t + \frac{1}{2}a\Delta t^2) & \Delta t \cos(\psi_k) & \frac{1}{2}\Delta t^2\cos(\psi_k) & 0\\ 0 & 1 & (v_k\Delta t + \frac{1}{2}a\Delta t^2)\cos(\psi_k) & \Delta t\sin(\psi_k) & \frac{1}{2}\Delta t^2\sin(\psi_k) & 0\\ 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & \Delta t & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 \end{matrix}\right\}JA=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧100000010000−sin(ψk)(vkΔt+21aΔt2)(vkΔt+21aΔt2)cos(ψk)1000Δtcos(ψk)Δtsin(ψk)010021Δt2cos(ψk)21Δt2sin(ψk)0Δt10000000⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫

对于HHH矩阵，根据观测到的xxx坐标，yyy坐标，偏航角得到：
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⎭⎬⎫

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(υ)

可以看出，噪声也是非线性的，对系统造成影响的主要是加速度的变化率和角加速度。为了便于计算，对上面的噪声方程，忽略角加速度对位移的影响，得到：
υ={16a.Δt3cos⁡(ψk)16a.Δt3sin⁡(ψk)12ω.Δt212a.Δt2a.Δtω.Δt}\upsilon=\left\{\begin{matrix} \frac{1}{6}\mathop{a}\limits^. \Delta t^3 \cos(\psi_k)\\ \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{a}\limits^.\Delta t^2\\ \mathop{a}\limits^. \Delta t\\ \mathop{\omega}\limits^. \Delta t \end{matrix}\right\} υ=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧61a.Δt3cos(ψk)61a.Δt3sin(ψk)21ω.Δt221a.Δt2a.Δtω.Δt⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫

则相应的GGG矩阵：

G={16Δt3cos⁡(ψk)016Δt3sin⁡(ψk)0012Δt212Δt20Δt00Δt}G = \left\{\begin{matrix} \frac{1}{6}\Delta t^3 \cos(\psi_k)& 0\\ \frac{1}{6}\Delta t^3 \sin(\psi_k) & 0\\ 0 & \frac{1}{2}\Delta t^2\\ \frac{1}{2}\Delta t^2 & 0\\ \Delta t & 0\\ 0 & \Delta t \end{matrix}\right\} G=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧61Δt3cos(ψk)61Δt3sin(ψk)021Δt2Δt00021Δt200Δt⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫

下面是各个参数矩阵的维度:
下面是各个参数矩阵的维度：

矩阵	A	B	Q	H	R	P	X	I	K	G	J_A
维度	6x6（与状态空间维度有关）	6×n（n为输入的列数）	6x6	3x6（3是z的维度，6是x的维度）	3x3（与z的维度有关）	6x6	6x1	6x6	6x3	6x2	6x6

自动驾驶算法-滤波器系列（四）——不同运动模型在KF/EKF中的应用相关推荐

自动驾驶算法-滤波器系列（三）——不同运动模型（CV、CA、CTRV、CTRA）的建模和推导
CV & CA & CTRV & CTRA 0. 运动模型简介 1. CV模型 2. CA模型 3. CTRV模型 4. CTRA模型上一篇文章主要讲解了不同卡尔曼滤波的原理 ...
自动驾驶算法-滤波器系列（二）—— 卡尔曼滤波简介及其变种（EKF、UKF、PF）介绍
KF&EKF&UKF&PF 1. 基础知识概要协方差矩阵多维高斯分布状态空间表达式 2. 什么是卡尔曼滤波器 3. 五个重要的公式公式介绍公式推导过程 4. 卡尔曼滤 ...
自动驾驶算法-滤波器系列（五）——高级运动模型在UKF中的应用
CTRV_UKF, CTRA_UKF 1. 基于CTRV的UKF 1.1 CTRV模型 1.2 UKF模型 2. 基于CTRA的UKF 2.1 CTRA模型扩展卡尔曼滤波算法是对非线性的系统方程或者 ...
自动驾驶算法-滤波器系列（八）——IMM交互多模型介绍
IMM交互多模型介绍 1. 简介 (1)IMM(Interacting Multiple Model) (2)马尔科夫概率转移矩阵 2. 算法流程 (1)输入交互(模型j) (2)卡尔曼滤波(模型j) ...
自动驾驶算法-滤波器系列（七）——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.限幅平均滤波 ...
自动驾驶算法-滤波器系列（一）——详解卡尔曼滤波原理
详解卡尔曼滤波原理什么是卡尔曼滤波? 我们能用卡尔曼滤波做什么? 卡尔曼滤波是如何看到你的问题的使用矩阵来描述问题外部控制量外部干扰用测量量来修正估计值融合高斯分布将所有公式整合起来总 ...
自动驾驶采标系列四：基于激光雷达的目标检测方法
标注猿的第55篇原创一个用数据视角看AI世界的标注猿上一篇文章我们讲了基于图像的目标检测技术,但对于标注人员来说这部分内容就相对比较难一些,只是作为一个了解就可以,但是如 ...
手撕自动驾驶算法——IMU测量模型、运动模型、误差模型
目录 IMU测量模型 IMU运动模型旋转量求导科氏加速度 IMU 误差模型确定性误差确定性误差误差标定六面法标定加速度六面法标定陀螺仪温度相关的参数标定随机误差高斯白噪声与随机游走 ...

自动驾驶算法-滤波器系列（四）——不同运动模型在KF/EKF中的应用