CTRV_UKF, CTRA_UKF

1. 基于CTRV的UKF
- 1.1 CTRV模型
- 1.2 UKF模型
2. 基于CTRA的UKF
- 2.1 CTRA模型

扩展卡尔曼滤波算法是对非线性的系统方程或者观测方程进行泰勒展开并保留其一阶近似项，这样会不可避免引入线性化误差。无迹卡尔曼滤波摒弃了对非线性函数进行线性化的传统做法，采用卡尔曼线性滤波框架，对于一步预测方程，使用无迹变换来处理均值和协方差的非线性传递问题。UKF算法是对非线性函数的概率密度分布进行近似，用一系列确定样本来逼近状态的后延概率密度，而不是对非线性函数进行近似，不需要对雅各比矩阵进行求导。UKF没有忽略高阶项，因此对于非线性分布的统计量有较高的计算精度，有效地克服了扩展卡尔曼滤波的估计精度低、稳定性差的缺陷。

1. 基于CTRV的UKF

1.1 CTRV模型

CTRV模型如下：
选取的状态空间为：
x=(x,y,v,ψ,ω)Tx=(x,y,v,\psi,\omega)^Tx=(x,y,v,ψ,ω)T
上式中变量依次为：x坐标，y坐标，速度，偏航角（与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

对于HHH矩阵，根据传感器采集到的数据（x坐标，y坐标，偏航角）以及状态空间的大小，确定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矩阵的初始化选择单位矩阵I5x5I_{5x5}I5x5初始化。RRR矩阵为测量噪声矩阵，采用对角矩阵初始化。QQQ矩阵的噪声参数也需要根据后期调参来确定。

1.2 UKF模型

UKF为无迹卡尔曼滤波，采用UT转换将状态转移方程线性化。本文档只有状态转移矩阵为非线性的，观测矩阵可以由要观测的数据直接得到，为线性的。故只需要对状态转移矩阵进行UT变换即可。

选取sigma点
计算2n+1个Sigma点，即采样点，这里的n为状态的维数，本文档中n为5.
X(0)=X−i=0X^{(0)} = \mathop{X}\limits^- \quad i=0X(0)=X−i=0
X(i)=X−+((n+λ)P)i,i=1∼nX^{(i)} = \mathop{X}\limits^- + (\sqrt{(n+\lambda)P)}_i, i=1 \sim nX(i)=X−+((n+λ)P)i,i=1∼n
X(i)=X−−((n+λ)P)i,i=n+1∼2nX^{(i)} = \mathop{X}\limits^- - (\sqrt{(n+\lambda)P)}_i, i=n+1 \sim 2nX(i)=X−−((n+λ)P)i,i=n+1∼2n
式中，PT(P)=P,(P)i\sqrt{P}^T(\sqrt{P}) = P, (\sqrt{P})_iPT(P)=P,(P)i表示矩阵方根的第i列。

X−\mathop{X}\limits^-X−取上一个状态的后验估计，取上一个状态的后验估计，取上一个状态的后验估计，P$为上一时刻的协方差矩阵。

设置权重值
ωm(n)=λn+λ\omega_m^{(n)} = \frac{\lambda}{n+\lambda}ωm(n)=n+λλ
ωc(n)=λn+λ+(1−α2+β)\omega_c^{(n)} = \frac{\lambda}{n+\lambda} + (1-\alpha^2+\beta)ωc(n)=n+λλ+(1−α2+β)
ωm(i)=ωc(i)=12(n+λ),i=1∼2n\omega_m^{(i)} = \omega_c^{(i)} = \frac{1}{2(n+\lambda)}, i = 1 \sim 2nωm(i)=ωc(i)=2(n+λ)1,i=1∼2n

式中，下标m为均值，c为协方差，上标为第几个采样点。参数λ=α2(n+κ)−n\lambda = \alpha^2(n + \kappa) -nλ=α2(n+κ)−n是一个缩放比例参数，用来降低总的预测误差，a的选取控制了采样点的分布状态，κ\kappaκ为待选参数，其具体数值虽然没有界限，但通常应确保矩阵(n+λ)P(n+\lambda)P(n+λ)P为半正定矩阵。待选参数β≥0\beta \ge 0β≥0是一个非负的权系数，它可以合并方程中高阶项的动差，这样就可以把高阶项的影响包括在内。要确保权重值之和为1。

预测，计算均值和协方差矩阵
计算系统状态量的一步预测及协方差矩阵。它由Sigma点集的预测值加权求和得到。将选取的Sigma点通过非线性系统得到预测的结果。UKF不同于传统的卡尔曼滤波算法，利用一组Sigma点的预测，并计算对它们加权求均值，得到系统状态量的一步预测。

X^(k+1∣k)=∑i=02nω(i)X(i)(k+1∣k)\hat{X}(k+1|k)=\sum_{i=0}^{2n}\omega^{(i)}X^{(i)}(k+1|k)X^(k+1∣k)=i=0∑2nω(i)X(i)(k+1∣k)P(k+1∣k)=∑i=02nω(i)[X^(k+1∣k)−X(i)(k+1∣k)][X^(k+1∣k)−X(i)(k+1∣k)]T+QP(k+1|k) = \sum_{i=0}^{2n}\omega^{(i)}[\hat{X}(k+1|k)-X^{(i)}(k+1|k)][\hat{X}(k+1|k)-X^{(i)}(k+1|k)]^T+QP(k+1∣k)=i=0∑2nω(i)[X^(k+1∣k)−X(i)(k+1∣k)][X^(k+1∣k)−X(i)(k+1∣k)]T+Q

计算卡尔曼增益矩阵，并更新状态和协方差
观测系统是线性的，故观测方程不需要线性化，直接将先验期望带入即可。

卡尔曼增益矩阵公式如下：
Kk=Pk−HT(HPk−HT+R)−1K_k={P_k^-}H^T(H{P_k^-}H^T+R)^{-1}Kk=Pk−HT(HPk−HT+R)−1

Pk−P_k^-Pk−即为上面得到的预测的协方差。

方差更新：
Pk=(I−KkH)Pk−P_k=(I-K_kH)P_k^-Pk=(I−KkH)Pk−

在传统UKF滤波算法中，需要对每个采样点进行非线性变换，计算量比较大，而且数值计算的误差也比较明显，估计误差协方差矩阵的非负定性和对称性会因此受到影响，从而影响滤波算法的收敛速度以及稳定性。为了提高滤波算法的滤波效率以及滤波精度，在递推运算过程中采用协方差矩阵的平方根来代替传统算法计算过程中的协方差矩阵，这种方法称为平方根无迹卡尔曼滤波算法。

2. 基于CTRA的UKF

2.1 CTRA模型

CTRA模型如下：
选取的状态空间为：

x=(x,y,ψ,v,a,ω)Tx=(x,y,\psi,v,a,\omega)^Tx=(x,y,ψ,v,a,ω)T

上式子中变量依次为：x坐标，y坐标，偏航角，速度，加速度，角速度。

状态方程为：
{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坐标，y坐标，偏航角）以及状态空间的大小，确定HHH矩阵：

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

针对QQQ矩阵和RRR矩阵中的未知参数，需要后期调参来确定。

QQQ矩阵为过程噪声的协方差矩阵，通过分析对系统的影响量，得到QQQ矩阵的数学表达式：

Q=cov(υ)=E[(Gu)(Gu)T]=GE[u2]GT=G{σak.200σω.2}GTQ=cov(\upsilon)=E[(Gu)(Gu)^T]=GE[u^2]G^T=G\left\{\begin{matrix}{\mathop{\sigma a_k}\limits^{.}}^2 & 0\\0 & {\sigma_{\mathop{\omega}\limits^{.}}}^2\end{matrix}\right\}G^TQ=cov(υ)=E[(Gu)(Gu)T]=GE[u2]GT=G{σak.200σω.2}GT

Q={16a.Δt3cos⁡(ψk+12ω.Δt2)16a.Δt3sin⁡(ψk+12ω.Δt2)12ω.Δt212a.Δt2a.Δtω.Δt}Q = \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\}Q=⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧61a.Δt3cos(ψk+21ω.Δt2)61a.Δt3sin(ψk+21ω.Δt2)21ω.Δt221a.Δt2a.Δtω.Δt⎭⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎪⎫

自动驾驶算法-滤波器系列（五）——高级运动模型在UKF中的应用相关推荐

自动驾驶算法-滤波器系列（二）—— 卡尔曼滤波简介及其变种（EKF、UKF、PF）介绍
KF&EKF&UKF&PF 1. 基础知识概要协方差矩阵多维高斯分布状态空间表达式 2. 什么是卡尔曼滤波器 3. 五个重要的公式公式介绍公式推导过程 4. 卡尔曼滤 ...
自动驾驶算法-滤波器系列（四）——不同运动模型在KF/EKF中的应用
不同运动模型应用于KF/EKF 1. 基于匀速(CV)运动模型的KF 2. 基于匀加速(CA)运动模型的KF 3. 基于CTRV的EKF 4. 基于CTRA的EKF 上一篇文章中,笔者针对四种不同的运 ...
自动驾驶算法-滤波器系列（三）——不同运动模型（CV、CA、CTRV、CTRA）的建模和推导
CV & CA & CTRV & CTRA 0. 运动模型简介 1. CV模型 2. CA模型 3. CTRV模型 4. 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.限幅平均滤波 ...
自动驾驶算法-滤波器系列（一）——详解卡尔曼滤波原理
详解卡尔曼滤波原理什么是卡尔曼滤波? 我们能用卡尔曼滤波做什么? 卡尔曼滤波是如何看到你的问题的使用矩阵来描述问题外部控制量外部干扰用测量量来修正估计值融合高斯分布将所有公式整合起来总 ...
中国AI芯片公司霸榜谷歌Waymo自动驾驶算法挑战赛！五个赛道，四项冠军
金磊发自凹非寺量子位报道 | 公众号 QbitAI 当今之世,最新证明自动驾驶实力的方法是什么? Waymo开放数据集挑战赛上一较高下,肯定是最具说服力的一种. 这不,作为全球自动驾驶公司王者 ...
手撕自动驾驶算法——IMU测量模型、运动模型、误差模型
目录 IMU测量模型 IMU运动模型旋转量求导科氏加速度 IMU 误差模型确定性误差确定性误差误差标定六面法标定加速度六面法标定陀螺仪温度相关的参数标定随机误差高斯白噪声与随机游走 ...

自动驾驶算法-滤波器系列（五）——高级运动模型在UKF中的应用