温故
分类 标签 归档 关于
温故

目录

运动学模型 (Kinematics Model)

自在即为仙 收录于 学术
   约 8311 字   预计阅读 17 分钟 

运动学模型 (Kinematics Model)主要整理车辆等常用运动模型。

Agreement

相关术语

1. 矩阵指数 / matrix exponential

在数学领域，矩阵指数是一种定义在方阵上的类似于普通指数函数的矩阵函数。它可以被用来求解线性微分方程组。此外，在李群理论中，它可以给出李代数和对应李群之间的关系。

对于 $x\in \mathbb{R}$，其指数函数 $e^x$ 可用多种等价的方式进行定义，特别是可用幂级数展开表示：

$e^x\mathrel{\vcenter{:}}= exp(x)\mathrel{\vcenter{:}}=\sum\limits^{\infty}_{k=0}\frac{x^k}{k!}$

相似的，对于大小为 $n\times n$ 的实（复）数方阵 $X$，其指数的数学定义可用如下幂级数表示： $e^X\mathrel{\vcenter{:}}= exp(X)\mathrel{\vcenter{:}}=\sum\limits^{\infty}_{k=0}\frac{1}{k!}X^k$

其中，$X^0$ 定义为与矩阵 $X$ 同维的单位阵。

此外，上述级数总是收敛的，因此 $e^X$ 的定义是良好的。

注意：如果 $X$ 是 $1 \times 1$ 矩阵，则其指数是一个由其唯一元素的普通指数构成的 $1 \times 1 $ 矩阵。

2. 三重积 / triple product

在几何和代数中，三重积是三个三维向量的乘积。三重积也称为混合积。通常有两种形式，即标量三重积和向量三重积。

2.1 标量三重积 / scalar triple product

对于三个向量 $\vec a$、$\vec b$、 $\vec c$，有标量三重积定义形式如下： $$ \begin{equation} \vec a\cdot (\vec b\times\vec c) \end{equation} $$

2.2 向量三重积 / vector triple product

对于三个向量 $\vec a$、$\vec b$、 $\vec c$，有向量三重积定义形式如下： $$ \begin{equation} \vec a\times (\vec b\times\vec c) \end{equation} $$

背景概念

1. 矩阵指数

更多关于矩阵指数的性质、概念等，请参考 矩阵指数。

1.1. 线性微分方程 / linear differential equation systems

更多关于常微分方程的解，请参考 常微分方程。

仅有积分上限的积分形式求解的是常数项恒为零的不定积分，可参考提问 What does an integral sign with only an upper limit mean?

对于一阶齐次线性微分方程 $\frac{\mathrm{d}y(t)}{\mathrm{d}t} = A(t)y(t)$，其通解为 $y(t) = e^{\int^{t}A(\tau)\mathrm{d}\tau+C}$。

对于一阶非齐次线性微分方程 $\frac{\mathrm{d}y(t)}{\mathrm{d}t} = A(t)y(t)+B(t)$，取积分因子1 $e^{-\int^{t}A(\lambda)\mathrm{d}\lambda}$，其通解为 $y(t) = e^{\int^{t}A(\lambda)\mathrm{d}\lambda}\left[\int^t e^{-\int^{\tau}A(\lambda)\mathrm{d}\lambda}B(\tau)\mathrm{d}\tau+C\right]$，其中 $C$ 为常数。

对于一阶常系数齐次线性微分方程 $\frac{\mathrm{d}y(t)}{\mathrm{d}t} = Ay(t)$，其通解为 $y(t) = C\cdot e^{At}$，其中 $C$ 为常数。

对于一阶常系数非齐次线性微分方程 $\frac{\mathrm{d}y(t)}{\mathrm{d}t} = Ay(t)+B(t)$，取积分因子 $e^{-\int^t A\mathrm{d}\lambda}$，其通解为 $y(t) = C\cdot e^{At}+\int^t e^{A(t-\tau)}B(\tau)\mathrm{d}\tau$，其中 $C$ 为常数。

对于给定初始条件的一阶常系数齐次线性微分方程组 $$ \begin{equation} \left\{ \begin{array}{l} \frac{\mathrm{d}y(t)}{\mathrm{d}t} &= Ay(t) \\ y(t_0) &= y_{t_0} \end{array} \right. \end{equation} $$ 其中 $A$ 为关于 $t$ 的常矩阵，则其有解 $$ \begin{equation} y(t) = y_{t_0}\cdot e^{At} \end{equation} $$ 对于给定初始条件的一阶常系数非齐次线性微分方程组 $$ \begin{equation} \left\{ \begin{array}{l} \frac{\mathrm{d}y(t)}{\mathrm{d}t} &= Ay(t)+B(t) \\ y(t_0) &= y_{t_0} \end{array} \right. \end{equation} $$ 其中 $A$ 为关于 $t$ 的常矩阵，则其有解 $$ \begin{equation} y(t) = y_{t_0}\cdot e^{A(t-t_0)}+\int^t_{t_0} e^{A(t-\tau)}B(\tau)d\tau \end{equation} $$

2. 三重积

更多关于三重积的性质、概念等，请参考。

2.1 标量三重积属性
  • 循环移位结果不变 $$ \begin{equation} \vec a\cdot (\vec b\times\vec c) = \vec b\cdot (\vec c\times\vec a) = \vec c\cdot (\vec a\times\vec b) \end{equation} $$
  • 交换运算符结果不变 $$ \begin{equation} \vec a\cdot (\vec b\times\vec c) = (\vec a\times\vec b)\cdot\vec c \end{equation} $$
  • 交换任意两个操作数结果变负 $$ \begin{equation} \begin{array}{l} \vec a\cdot (\vec b\times\vec c) &= -\vec a\cdot (\vec c\times\vec b) \\ &= -\vec b\cdot (\vec a\times\vec c) \\ &= -\vec c\cdot (\vec b\times\vec a) \end{array} \end{equation} $$
  • 行列式表示标量三重积 $$ \begin{equation} \begin{array}{l} \vec a\cdot (\vec b\times\vec c) &= \det\left[\begin{array}{ccc} a_1 & a_2 & a_3\\ b_1 & b_2 & b_3\\ c_1 & c_2 & c_3\end{array}\right] \\ &= \det\left[\begin{array}{ccc} a_1 & b_1 & c_1\\ a_2 & b_2 & c_2\\ a_3 & b_3 & c_3\end{array}\right] = \det\left[\begin{array}{ccc}\vec a & \vec b & \vec c\end{array}\right] = \left|\begin{array}{ccc}\vec a & \vec b & \vec c\end{array}\right| \end{array} \end{equation} $$
2.2 向量三重积属性
  • 向量三重积展开式

记忆法：括号外的矢量在哪边，括号内靠近那边的矢量所在的项前面就是正号，另一项前面则是负号

证明思路：分解成基底并展开化简。更多证明思路，可以参考《三矢量叉乘展开成点乘的公式如何证明？》 $$ \begin{equation} \vec a\times (\vec b\times\vec c) = \vec b(\vec a\cdot \vec c) - \vec c(\vec a\cdot \vec b) \end{equation} $$

  • 交换操作数 $$ \begin{equation} (\vec b\times\vec c)\times\vec a = -\vec a\times (\vec b\times\vec c) = -\vec b(\vec a\cdot \vec c) + \vec c(\vec a\cdot \vec b) \end{equation} $$
3. 三维旋转角速度 / angular velocity

更多关于三维旋转的概念、推导，请参考《State Estimation for Robotics》的6.2.4节。

该书另有中文译本草稿，请参考《State Estimation for Robotics-CN》。

现有两个参考系 $\vec{\mathcal F_A}$ 和 $\vec{\mathcal F_B}$，其中 $\vec{\mathcal F_B}$ 以旋转速度 $\vec \omega_{BA}$ 相对 $\vec{\mathcal F_A}$ 旋转（以旋转速率 $\lVert\vec \omega_{BA}\rVert$ 绕瞬时旋转轴 $\vec \omega_{BA}$ 旋转），同样的，$\vec{\mathcal F_A}$ 以旋转速度 $\vec \omega_{AB}$ 相对 $\vec{\mathcal F_B}$ 旋转，且 $\vec \omega_{BA}=-\vec \omega_{AB}$。

显然，一个向量在两个坐标系中观测到的运动现象是不同的。因此记任意向量 $\vec r$ 在参考系 $\vec{\mathcal F_A}$ 下对时间的导数为符号 $\dot{\overrightarrow {r^A}}$；同理，符号 $\dot {\overrightarrow{r^B}}$ 表示其在参考系 $\vec{\mathcal F_B}$ 下对时间的导数；特别的，$\dot{\overrightarrow {\mathcal F^B_{A_x}}}$ 表示 $\vec{\mathcal F_A}$ 参考系的 $x$ 轴在 $\vec{\mathcal F_B}$ 参考下下对时间的导数，而 $\dot{\overrightarrow {\mathcal F^B_A}}$ 表示 $\vec{\mathcal F_A}$ 参考系自身（所有基向量）在 $\vec{\mathcal F_B}$ 参考下下对时间的导数。

因为各参考系相对自身都是静止的，因此有 $\dot{\overrightarrow {\mathcal F^A_A}}=\vec 0$ 和 $\dot{\overrightarrow {\mathcal F^B_B}}=\vec 0$。

3.1. 不同参考系间的时间导数

参考系 $\vec{\mathcal F_B}$ 的轴 $\overrightarrow{\mathcal F_{B_x}}$ 相对于参考系 $\vec{\mathcal F_A}$ （简记为 $\overrightarrow{B^A_x}$）的时间导数 $$ \begin{equation} \left\Vert\dot{\overrightarrow {\mathcal F^A_{B_x}}}\right\Vert = \frac{\left\Vert\mathrm{d}\overrightarrow{B^A_x}\right\Vert}{\mathrm{d}t} =\ \lim\limits_{\Delta t\to 0}\frac{\Vert\Delta\overrightarrow{B^A_x}\Vert}{\Delta t} =\ \lim\limits_{\Delta t\to 0}\frac{2\Vert\overrightarrow{\mathcal F_{B_x}}\Vert \sin(\frac{\Vert\vec \omega_{BA}\Vert\Delta t}{2})}{\Delta t} =\ \Vert\vec \omega_{BA}\Vert\cdot\Vert\overrightarrow{\mathcal F_{B_x}}\Vert \end{equation} $$ 显然，该时间导数的方向垂直于旋转轴 $\vec \omega_{BA}$ 和轴 $\overrightarrow{\mathcal F_{B_x}}$，因此 $$ \begin{equation} \dot{\overrightarrow {\mathcal F^A_{B_x}}} = \ \vec \omega_{BA}\times \overrightarrow{\mathcal F_{B_x}} \end{equation} $$ 同理，对于轴 $\overrightarrow{\mathcal F_{B_y}}$ 和 $\overrightarrow{\mathcal F_{B_z}}$ 同样满足类似关系，因此有 $$ \begin{equation} \left[\begin{array}{ccc}\dot{\overrightarrow {\mathcal F^A_{B_x}}} & \dot{\overrightarrow {\mathcal F^A_{B_y}}} & \dot{\overrightarrow {\mathcal F^A_{B_z}}}\end{array}\right] = \vec \omega_{BA}\times\left[\begin{array}{ccc}\overrightarrow{\mathcal F_{B_x}} & \overrightarrow{\mathcal F_{B_y}} & \overrightarrow{\mathcal F_{B_z}}\end{array}\right] \end{equation} $$ 即 $$ \begin{equation} \dot{\overrightarrow {\mathcal F^A_B}}^T = \ \vec \omega_{BA}\times \overrightarrow{\mathcal F_B}^T \end{equation} $$

3.2. 空间任意向量在不同参考系下的时间导数

对于空间任意向量 $\vec r$，可以在两个参考下分别使用对应单位向量表示，即 $$ \vec r \mathrel{\vcenter{:}}= r_A\mathcal F^T_A \mathrel{\vcenter{:}}= r_B\mathcal F^T_B $$ 则有 $$ \dot{\overrightarrow {r^A}} = \dot{r^A_A}\mathcal F^T_A+r_A\dot{\overrightarrow {\mathcal F^A_A}}^T = \dot{r^A_A}\mathcal F^T_A = \dot{r_A}\mathcal F^T_A $$ 同理 $$ \dot{\overrightarrow {r^B}} = \dot{r^B_B}\mathcal F^T_B+r_B\dot{\overrightarrow {\mathcal F^B_B}}^T = \dot{r^B_B}\mathcal F^T_B = \dot{r_B}\mathcal F^T_B $$ 此外，也可以用 $\vec{\mathcal F_B}$ 参考系中的向量表示相对于参考系 $\vec{\mathcal F_A}$ 的时间导数 $$ \begin{align} \dot{\overrightarrow {r^A}} &= \dot{r^A_B}\mathcal F^T_B+r_B\dot{\overrightarrow {\mathcal F^A_B}}^T \\ &= \dot{r_B}\mathcal F^T_B+\vec \omega_{BA}\times r_B\overrightarrow{\mathcal F_B}^T \\ &= \dot{\overrightarrow {r^B}}+\vec \omega_{BA}\times\vec r \end{align} $$ 就实际应用来讲，上述公式刻画了任意向量 $\vec r$ 在惯性系 $\vec{\mathcal F_A}$ 下对时间的导数与运动系 $\vec{\mathcal F_B}$ 下对时间的导数之间的关系。

常见的运动模型

准备

三角函数

对于三角函数，如果 $\Delta \theta$ 足够小，则存在如下近似 $$ \begin{align} cos(\theta+\Delta\theta) &= cos(\theta)cos(\Delta\theta) - sin(\theta)sin(\Delta\theta) \\ &\approx cos(\theta) - sin(\theta)\Delta\theta \tag{0.1} \\ sin(\theta+\Delta\theta) &= sin(\theta)cos(\Delta\theta) + cos(\theta)sin(\Delta\theta) \\ &\approx sin(\theta) + cos(\theta)\Delta\theta \tag{0.2} \end{align} $$

车辆运动模型之单车模型

简单的单车运动学模型如下所示。

https://store.yirami.xyz/review/kinematics_model/bicycle_model.png

引用自AdamShan博客，侵删。

其中 $l_f$ 和 $l_r$ 分别为前后轮到重心的距离，

该模型定义4个状态变量，意义如下

  • $x, y$ 表征车辆位置
  • $\psi$ 表征车辆偏航角，即 Yaw 角度
  • $v$ 表征车辆速度

则有状态向量 $$ \begin{equation} \overrightarrow{X} = \left[ \begin{array}{ccccc} x & y & \psi & v \end{array} \right]^T \tag{0.3}\\ \end{equation} $$ 状态转移方程（此处存疑，需查证） $$ \begin{equation} \overrightarrow{X_{k}} = \overrightarrow{X_{k-1}} + \left[ \begin{array}{c} v_{k-1}\cos\left(\psi_{k-1}+\beta\right)\Delta t \\ v_{k-1}\sin\left(\psi_{k-1}+\beta\right)\Delta t \\ \frac{v_{k-1}}{l_r}\sin\left(\beta\right)\Delta t \\ a\Delta t \end{array} \right] \tag{0.4} \end{equation} $$ 其中 $$ \begin{equation} \beta = \tan^{-1}\left(\frac{l_r}{l_f+l_r}\tan\left(\delta _f\right)\right) \end{equation} $$

大部分汽车都是前轮转向，因此 $\delta_r=0$.

运动模型间的转换关系

stateDiagram-v2 CCA/CSAA --> CA: ψ=c=0 CA --> CV: a=0 CCA/CSAA --> CSAV: a=0 CSAV --> CV: ψ=0 CTRA --> CA: ω=0 CTRA --> CTRV: a=0 CTRV --> CV: ω=0

1. Constant Velocity (CV) in 2D

该模型假设目标的运动具备如下约束：

  1. 目标在平面运动
  2. 目标做直线运动
  3. 目标运动的速度不变

基于上述假设，可以将目标在极小时间片内的运动近似为匀速直线运动。

2. Constant Acceleration (CA) in 2D / (Aptiv) Cartesian Constant Acceleration (CCA) in 2D

该模型假设目标的运动具备如下约束：

  1. 目标在平面运动
  2. 目标做直线运动
  3. 目标运动的加速度不变

基于上述假设，可以将目标在极小时间片内的运动近似为匀加速直线运动。

选择状态向量 $\left[x\ \dot x\ \ddot x\ y\ \dot y\ \ddot y\right]^T$

定义模型的状态向量为$\overrightarrow{X}$，分别由位置 $x$、分速度 $\dot x$、分加速度 $\ddot x$、位置 $y$、分速度 $\dot y$、分加速度 $\ddot y$组成。 $$ \begin{equation} \overrightarrow{X} = \left[ \begin{array}{ccccc} x & \dot x & \ddot x & y & \dot y & \ddot y \end{array} \right]^T \tag{2.1}\\ \end{equation} $$ 其一阶导$\dot{\overrightarrow{X}}$为 $$ \begin{equation} \dot{\overrightarrow{X}} = \left[ \begin{array}{c} \dot{x} = \dot{x} \\ \ddot{x} = \ddot{x} \\ \dddot{x} = 0 \\ \dot{y} = \dot{y} \\ \ddot{y} = \ddot{y} \\ \dddot{y} = 0 \end{array} \right] \tag{2.2} \end{equation} $$ 若模型存在一定的不确定性，即存在由 $\ddot{x}$ 和 $\ddot{y}$ 引起的过程噪声 $V_{\ddot x }$ 和 $V_{\ddot y}$，且满足 $V_{\ddot{x}}\sim N\left(0, \sigma_{\ddot{x}}^2\right)$ 和 $V_{\ddot{y}}\sim N\left(0, \sigma_{\ddot{y}}^2\right)$，则状态向量的一阶导可修正为 $$ \begin{equation} \dot{\overrightarrow{X}} = \left[ \begin{array}{c} \dot x \\ \ddot x \\ 0 \\ \dot y \\ \ddot y \\ 0 \end{array} \right] + \left[ \begin{array}{c} 0 \\ 0 \\ V_{\ddot x}\mathrel{\vcenter{:}}=\eta \\ 0 \\ 0 \\ V_{\ddot y}\mathrel{\vcenter{:}}=\zeta \end{array} \right]\tag{2.3} \end{equation} $$ 上述方程可表述为一阶线性微分方程 $$ \begin{align} \dot{\overrightarrow{X}} &= \left[\begin{array}{cccccc} 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 & 0 & 0 \end{array}\right]\overrightarrow{X}+ \left[\begin{array}{cc} 0 & 0 \\ 0 & 0 \\ 1 & 0 \\ 0 & 0 \\ 0 & 0 \\ 0 & 1\end{array}\right]\left[\begin{array}{c} \eta \\\zeta\end{array}\right] \tag{2.4} \end{align} $$ 则该一阶常系数非齐次线性微分方程在时间区间 $[t_0,t)$ 上的解为 $$ \begin{align} \overrightarrow{X} &= e^{\left(\left[\begin{array}{cccccc} 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 & 0 & 0 \end{array}\right](t-t_0)\right)}\cdot\overrightarrow{X}(t_0) +\int^t_{t_0}e^{\left(\left[\begin{array}{cccccc} 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 & 0 & 0 \end{array}\right](t-\tau)\right)}\left[\begin{array}{cc} 0 & 0 \\ 0 & 0 \\ 1 & 0 \\ 0 & 0 \\ 0 & 0 \\ 0 & 1\end{array}\right]\left[\begin{array}{c} \eta(\tau) \\\zeta(\tau)\end{array}\right]d\tau \notag\\ &= \left[\begin{array}{cccccc} 1 & t-t_0 & \frac{(t-t_0)^2}{2} & 0 & 0 & 0 \\ 0 & 1 & t-t_0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & t-t_0 & \frac{(t-t_0)^2}{2} \\ 0 & 0 & 0 & 0 & 1 & t-t_0 \\ 0 & 0 & 0 & 0 & 0 & 1\end{array}\right]\cdot\overrightarrow{X}(t_0)+ \int^t_{t_0}\left[\begin{array}{c} \eta(\tau)\cdot\frac{(t-\tau)^2}{2} \\ \eta(\tau)\cdot(t-\tau) \\ \eta(\tau) \\ \zeta(\tau)\cdot\frac{(t-\tau)^2}{2} \\ \zeta(\tau)\cdot(t-\tau) \\ \zeta(\tau)\end{array}\right]d\tau \notag \\ &= \left[\begin{array}{c} x(t_0)+\dot{x}(t_0)\cdot(t-t_0)+\ddot{x}(t_0)\cdot\frac{(t-t_0)^2}{2} \\ \dot{x}(t_0)+\ddot{x}(t_0)\cdot(t-t_0) \\ \ddot{x}(t_0) \\ y(t_0)+\dot{y}(t_0)\cdot(t-t_0)+\ddot{y}(t_0)\cdot\frac{(t-t_0)^2}{2} \\ \dot{y}(t_0)+\ddot{y}(t_0)\cdot(t-t_0) \\ \ddot{y}(t_0)\end{array}\right]+ \left[\begin{array}{c} \eta(t_0)\cdot\frac{(t-t_0)^3}{6} \\ \eta(t_0)\cdot\frac{(t-t_0)^2}{2} \\ \eta(t_0)\cdot(t-t_0) \\ \zeta(t_0)\cdot\frac{(t-t_0)^6}{3} \\ \zeta(t_0)\cdot\frac{(t-t_0)^2}{2} \\ \zeta(t_0)\cdot(t-t_0) \end{array}\right] \tag{2.5} \end{align} $$ 其中根据简化计算假设在时间区间 $[t_0,t)$ 上模型的过程噪声为常量，即 $\eta(\tau)$ 和 $\zeta(\tau)$ 是常量。

提示：

矩阵指数的计算可以借助用于符号计算的开源 Python 库 SymPy，如

import sympy
from sympy import init_printing
init_printing(use_latex=True)

t, t0 = sympy.symbols("t t_0")
A = sympy.Matrix([[0, t-t0, 0, 0, 0, 0], [0, 0, t-t0, 0, 0, 0], [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, t-t0, 0], [0, 0, 0, 0, 0, t-t0], [0, 0, 0, 0, 0, 0]])
eA = sympy.simplify(sympy.exp(A))
print(f"The exponential of A is: \n\n{sympy.pretty(eA)}")

3. Constant Turn Rate and Velocity (CTRV) in 2D

该模型假设目标的运动具备如下约束：

  1. 目标在平面内运动
  2. 目标运动的转向速率不变
  3. 目标运动的速率不变

基于上述假设，可以将目标在极小时间片内的运动近似为匀速圆周运动。

CTRV示意图
选择状态向量 $\left[x\ y\ v\ \theta\ \dot{\theta}\right]^T$

定义模型的状态向量为$\overrightarrow{X}$，分别由位置 $x$、位置 $y$、切向速率 $v$、横摆角 $\theta$、横摆角速率 $\dot{\theta}$组成。 $$ \begin{equation} \overrightarrow{X} = \left[ \begin{array}{ccccc} x & y & v & \theta & \dot{\theta} \end{array} \right]^T \tag{3.1}\\ \end{equation} $$ 其一阶导$\dot{\overrightarrow{X}}$为 $$ \begin{equation} \dot{\overrightarrow{X}} = \left[ \begin{array}{c} \dot{x} = v\cdot cos\theta \\ \dot{y} = v\cdot sin\theta \\ \dot{v} = 0 \\ \dot{\theta} \\ \ddot{\theta} = 0 \end{array} \right] \tag{3.2} \end{equation} $$ 对$\dot{\overrightarrow{X}}$积分（令 $t_k-t_{k-1}=\Delta t$），可得状态转移方程 $\left(\dot{\theta}_{t_{k-1}} \equiv \dot{\theta} \neq 0\right)$ 如下2 $$ \begin{align} \overrightarrow{X_{t_k}} &= \overrightarrow{X_{t_{k-1}}} + \left[ \begin{array}{c} \int_{t_{k-1}}^{t_k}\left(v\cdot cos\theta\right)dt = v\int_{t_{k-1}}^{t_k}\left(cos(\theta_{t_{k-1}}+\dot{\theta}_{t_{k-1}}(t_k-t_{k-1}))\right)dt \\ \int_{t_{k-1}}^{t_k}\left(v\cdot sin\theta\right)dt = v\int_{t_{k-1}}^{t_k}\left(sin(\theta_{t_{k-1}}+\dot{\theta}_{t_{k-1}}(t_k-t_{k-1}))\right)dt \\ 0 \\ \int_{t_{k-1}}^{t_k}\dot{\theta}dt = \dot{\theta}_{t_{k-1}}(t_k-t_{k-1}) = \dot{\theta}_{t_{k-1}}\Delta t \\ 0 \end{array} \right] \notag \\ &= \overrightarrow{X_{t_{k-1}}} + \left[ \begin{array}{c} \frac{v_{t_{k-1}}}{\dot{\theta}_{t_{k-1}}}\left(\sin(\dot{\theta}_{t_{k-1}}\Delta t+\theta_{t_{k-1}})-\sin(\theta_{t_{k-1}})\right) \\ \frac{v_{t_{k-1}}}{\dot{\theta}_{t_{k-1}}}\left(-\cos(\dot{\theta}_{t_{k-1}}\Delta t+\theta_{t_{k-1}})+\cos(\theta_{t_{k-1}})\right) \\ 0 \\ \dot{\theta}_{t_{k-1}}\Delta t \\ 0 \end{array} \right] \tag{3.3} \end{align} $$ 在 $\left(\dot{\theta}_{t_{k-1}}=0\right)$ 时状态转移方程如下 $$ \begin{equation} \overrightarrow{X_{t_k}} = \overrightarrow{X_{t_{k-1}}} + \left[ \begin{array}{c} v_{t_{k-1}}\cos\theta_{t_{k-1}}\Delta t \\ v_{t_{k-1}}\sin\theta_{t_{k-1}}\Delta t \\ 0 \\ \dot{\theta}_{t_{k-1}}\Delta t = 0 \\ 0 \end{array} \right] \tag{3.4} \end{equation} $$ 此外，对于CTRV模型还存在由径向加速度 $\dot{v}$ 和角加速度 $\ddot{\theta}$ 引起的过程噪声 (process noise)，分别表示为$V_{\dot{v}}\sim N\left(0, \sigma_{\dot{v}}^2\right)$和$V_{\ddot{\theta}}\sim N\left(0, \sigma_{\ddot{\theta}}^2\right)$，向量表示为 $$ \begin{equation} \left[ \begin{array}{c} V_{\dot{v},t_k} \\ V_{\ddot{\theta},t_k} \end{array} \right] \tag{3.5} \end{equation} $$ 该过程噪声对状态向量的影响可表示为 $$ \begin{equation} \left[ \begin{array}{c} \frac{1}{2}\Delta t^2V_{\dot{v},t_{k-1}}\cos\theta \\ \frac{1}{2}\Delta t^2V_{\dot{v},t_{k-1}}\sin\theta \\ \Delta tV_{\dot{v},t_{k-1}} \\ \frac{1}{2}\Delta t^2V_{\ddot{\theta},t_{k-1}} \\ \Delta tV_{\ddot{\theta},t_{k-1}} \end{array} \right] \tag{3.6} \end{equation} $$

选择状态向量 $\left[x\ \dot x\ \ddot x\ y\ \dot y\ \ddot y\right]^T$
法一

定义模型的状态向量为 $\overrightarrow{X}$，分别由位置 $x$、分速度 $\dot x$、分加速度 $\ddot x$、位置 $y$、分速度 $\dot y$、分加速度 $\ddot y$组成。 $$ \begin{equation} \overrightarrow{X} = \left[ \begin{array}{ccccc} x & \dot x & \ddot x & y & \dot y & \ddot y \end{array} \right]^T \tag{3.7}\\ \end{equation} $$ 其一阶导 $\dot{\overrightarrow{X}}$ 为 $$ \begin{equation} \dot{\overrightarrow{X}} = \left[ \begin{array}{c} \dot x = v\cdot cos\theta \\ \ddot x = -v\dot \theta\cdot sin\theta \\ \dddot x = -v\dot \theta^2\cdot cos\theta \\ \dot y = v\cdot sin\theta \\ \ddot y = v\dot \theta\cdot cos\theta \\ \dddot y = -v\dot \theta^2\cdot sin\theta \end{array} \right] \tag{3.8} \end{equation} $$ 对 $\dot{\overrightarrow{X}}$ 积分（令 $t_k-t_{k-1}=\Delta t$），可得状态转移方程 $\left(\dot{\theta}_{t_{k-1}} \equiv \dot{\theta} \neq 0\right)$ 如下 $$ \begin{align} \overrightarrow{X_{t_k}} &= \overrightarrow{X_{t_{k-1}}} + \left[ \begin{array}{c} \int_{t_{k-1}}^{t_k}\left(v\cdot cos\theta\right)dt = v\int_{t_{k-1}}^{t_k}\left(cos(\theta_{t_{k-1}}+\dot{\theta}_{t_{k-1}}(t_k-t_{k-1}))\right)dt \\ \int_{t_{k-1}}^{t_k}\left(-v\dot \theta\cdot sin\theta\right)dt = -v\dot \theta\int_{t_{k-1}}^{t_k}\left(sin(\theta_{t_{k-1}}+\dot{\theta}_{t_{k-1}}(t_k-t_{k-1}))\right)dt \\ \int_{t_{k-1}}^{t_k}\left(-v\dot \theta^2\cdot cos\theta\right)dt = -v\dot \theta^2\int_{t_{k-1}}^{t_k}\left(cos(\theta_{t_{k-1}}+\dot{\theta}_{t_{k-1}}(t_k-t_{k-1}))\right)dt \\ \int_{t_{k-1}}^{t_k}\left(v\cdot sin\theta\right)dt = v\int_{t_{k-1}}^{t_k}\left(sin(\theta_{t_{k-1}}+\dot{\theta}_{t_{k-1}}(t_k-t_{k-1}))\right)dt \\ \int_{t_{k-1}}^{t_k}\left(v\dot \theta\cdot cos\theta\right)dt = v\dot \theta\int_{t_{k-1}}^{t_k}\left(cos(\theta_{t_{k-1}}+\dot{\theta}_{t_{k-1}}(t_k-t_{k-1}))\right)dt \\ \int_{t_{k-1}}^{t_k}\left(-v\dot \theta^2\cdot sin\theta\right)dt = -v\dot \theta^2\int_{t_{k-1}}^{t_k}\left(sin(\theta_{t_{k-1}}+\dot{\theta}_{t_{k-1}}(t_k-t_{k-1}))\right)dt \end{array} \right] \notag \\ &= \overrightarrow{X_{t_{k-1}}} + \left[ \begin{array}{c} \frac{v_{t_{k-1}}}{\dot{\theta}_{t_{k-1}}}\left(\sin(\dot{\theta}_{t_{k-1}}\Delta t+\theta_{t_{k-1}})-\sin(\theta_{t_{k-1}})\right) \\ -v_{t_{k-1}}\left(-\cos(\dot{\theta}_{t_{k-1}}\Delta t+\theta_{t_{k-1}})+\cos(\theta_{t_{k-1}})\right) \\ -v_{t_{k-1}}\dot{\theta}\left(\sin(\dot{\theta}_{t_{k-1}}\Delta t+\theta_{t_{k-1}})-\sin(\theta_{t_{k-1}})\right) \\ \frac{v_{t_{k-1}}}{\dot{\theta}_{t_{k-1}}}\left(-\cos(\dot{\theta}_{t_{k-1}}\Delta t+\theta_{t_{k-1}})+\cos(\theta_{t_{k-1}})\right) \\ v_{t_{k-1}}\left(\sin(\dot{\theta}_{t_{k-1}}\Delta t+\theta_{t_{k-1}})-\sin(\theta_{t_{k-1}})\right) \\ -v_{t_{k-1}}\dot{\theta}\left(-\cos(\dot{\theta}_{t_{k-1}}\Delta t+\theta_{t_{k-1}})+\cos(\theta_{t_{k-1}})\right) \end{array} \right] \notag \\ &= \overrightarrow{X_{t_{k-1}}} + \left[ \begin{array}{c} \frac{v_{t_{k-1}}}{\dot{\theta}_{t_{k-1}}}\left(\sin(\dot{\theta}_{t_{k-1}}\Delta t)\cos(\theta_{t_{k-1}})+\cos(\dot{\theta}_{t_{k-1}}\Delta t)\sin(\theta_{t_{k-1}})-\sin(\theta_{t_{k-1}})\right) \\ v_{t_{k-1}}\left(\cos(\dot{\theta}_{t_{k-1}}\Delta t)\cos(\theta_{t_{k-1}})-\sin(\dot{\theta}_{t_{k-1}}\Delta t)\sin(\theta_{t_{k-1}})-\cos(\theta_{t_{k-1}})\right) \\ -v_{t_{k-1}}\dot{\theta}\left(\sin(\dot{\theta}_{t_{k-1}}\Delta t)\cos(\theta_{t_{k-1}})+\cos(\dot{\theta}_{t_{k-1}}\Delta t)\sin(\theta_{t_{k-1}})-\sin(\theta_{t_{k-1}})\right) \\ -\frac{v_{t_{k-1}}}{\dot{\theta}_{t_{k-1}}}\left(\cos(\dot{\theta}_{t_{k-1}}\Delta t)\cos(\theta_{t_{k-1}})-\sin(\dot{\theta}_{t_{k-1}}\Delta t)\sin(\theta_{t_{k-1}})-\cos(\theta_{t_{k-1}})\right) \\ v_{t_{k-1}}\left(\sin(\dot{\theta}_{t_{k-1}}\Delta t)\cos(\theta_{t_{k-1}})+\cos(\dot{\theta}_{t_{k-1}}\Delta t)\sin(\theta_{t_{k-1}})-\sin(\theta_{t_{k-1}})\right) \\ v_{t_{k-1}}\dot{\theta}\left(\cos(\dot{\theta}_{t_{k-1}}\Delta t)\cos(\theta_{t_{k-1}})-\sin(\dot{\theta}_{t_{k-1}}\Delta t)\sin(\theta_{t_{k-1}})-\cos(\theta_{t_{k-1}})\right) \end{array} \right] \notag \\ &= \overrightarrow{X_{t_{k-1}}} + \left[ \begin{array}{c} \frac{v_{t_{k-1}}}{\dot{\theta}_{t_{k-1}}}\left(\sin(\dot{\theta}_{t_{k-1}}\Delta t)\frac{\dot x}{v_{t_{k-1}}}-\cos(\dot{\theta}_{t_{k-1}}\Delta t)\frac{\ddot x}{v_{t_{k-1}}\dot{\theta}}+\frac{\ddot x}{v_{t_{k-1}}\dot{\theta}}\right) \\ v_{t_{k-1}}\left(\cos(\dot{\theta}_{t_{k-1}}\Delta t)\frac{\dot x}{v_{t_{k-1}}}+\sin(\dot{\theta}_{t_{k-1}}\Delta t)\frac{\ddot x}{v_{t_{k-1}}\dot{\theta}}-\frac{\dot x}{v_{t_{k-1}}}\right) \\ -v_{t_{k-1}}\dot{\theta}\left(\sin(\dot{\theta}_{t_{k-1}}\Delta t)\frac{\dot x}{v_{t_{k-1}}}-\cos(\dot{\theta}_{t_{k-1}}\Delta t)\frac{\ddot x}{v_{t_{k-1}}\dot{\theta}}+\frac{\ddot x}{v_{t_{k-1}}\dot{\theta}}\right) \\ -\frac{v_{t_{k-1}}}{\dot{\theta}_{t_{k-1}}}\left(\cos(\dot{\theta}_{t_{k-1}}\Delta t)\frac{\ddot y}{v_{t_{k-1}}\dot{\theta}}-\sin(\dot{\theta}_{t_{k-1}}\Delta t)\frac{\dot y}{v_{t_{k-1}}}-\frac{\ddot y}{v_{t_{k-1}}\dot{\theta}}\right) \\ v_{t_{k-1}}\left(\sin(\dot{\theta}_{t_{k-1}}\Delta t)\frac{\ddot y}{v_{t_{k-1}}\dot{\theta}}+\cos(\dot{\theta}_{t_{k-1}}\Delta t)\frac{\dot y}{v_{t_{k-1}}}-\frac{\dot y}{v_{t_{k-1}}}\right) \\ v_{t_{k-1}}\dot{\theta}\left(\cos(\dot{\theta}_{t_{k-1}}\Delta t)\frac{\ddot y}{v_{t_{k-1}}\dot{\theta}}-\sin(\dot{\theta}_{t_{k-1}}\Delta t)\frac{\dot y}{v_{t_{k-1}}}-\frac{\ddot y}{v_{t_{k-1}}\dot{\theta}}\right) \end{array} \right] \notag \\ &= \left[ \begin{array}{cccccc} 1 & \frac{\sin(\dot{\theta}_{t_{k-1}}\Delta t)}{\dot \theta} & \frac{1-\cos(\dot{\theta}_{t_{k-1}}\Delta t)}{\dot \theta^2} & 0 & 0 & 0 \\ 0 & \cos(\dot{\theta}_{t_{k-1}}\Delta t) & \frac{\sin(\dot{\theta}_{t_{k-1}}\Delta t)}{\dot \theta} & 0 & 0 & 0 \\ 0 & -\dot \theta\sin(\dot{\theta}_{t_{k-1}}\Delta t) & \cos(\dot{\theta}_{t_{k-1}}\Delta t) & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & \frac{\sin(\dot{\theta}_{t_{k-1}}\Delta t)}{\dot \theta} & \frac{1-\cos(\dot{\theta}_{t_{k-1}}\Delta t)}{\dot \theta^2} \\ 0 & 0 & 0 & 0 & \cos(\dot{\theta}_{t_{k-1}}\Delta t) & \frac{\sin(\dot{\theta}_{t_{k-1}}\Delta t)}{\dot \theta} \\ 0 & 0 & 0 & 0 & -\dot \theta\sin(\dot{\theta}_{t_{k-1}}\Delta t) & \cos(\dot{\theta}_{t_{k-1}}\Delta t) \end{array} \right] \cdot \overrightarrow{X_{t_{k-1}}} \tag{3.9} \end{align} $$ 在 $\left(\dot{\theta}_{t_{k-1}}=0\right)$ 时状态转移方程如下 $$ \begin{align} \overrightarrow{X_{t_k}} &= \overrightarrow{X_{t_{k-1}}} + \left[ \begin{array}{c} v_{t_{k-1}}\cos\theta_{t_{k-1}}\Delta t = \dot x\Delta t \\ 0 \\ 0 \\ v_{t_{k-1}}\sin\theta_{t_{k-1}}\Delta t = \dot y\Delta t \\ 0 \\ 0 \end{array} \right] \notag \\ &= \left[ \begin{array}{cccccc} 1 & \Delta t & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 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 & 1) \end{array} \right]\cdot\overrightarrow{X_{t_{k-1}}} \tag{3.10} \end{align} $$

法二

可以基于角速度定义推导出同样的结论，在[下一节](#Fixed-Center Constant Angular-Velocity (CAV) in 3D)中将推导更一般的情形（三维空间）下目标运动的状态方程。

4. Fixed-Center Constant Angular-Velocity (CAV) in 3D

该模型假设目标的运动具备如下约束：

  1. 目标绕定点运动
  2. 目标运动的转向速率不变

基于上述假设，同样可以将目标在极小时间片内的运动近似为匀速圆周运动。

PS：本模型推导可参考《Survey of maneuvering target tracking. Part I. Dynamic models》一文中的6.1.1节

目标在参考系中的位置用 $\vec p$ 表示，其瞬时绕点 $\overrightarrow{p_0}$ 以角速度 $\vec\omega$ 旋转。依据上述假设，则有 $$ \begin{equation} \left\{ \begin{array}{l} \dot{\overrightarrow{p_0}} &= \vec 0 \\ \dot{\vec\omega} &= \vec 0 \end{array} \right. \tag{4.1} \end{equation} $$ 则向量 $\left(\vec p-\overrightarrow{p_0}\right)$ 对时间的导数为目标速度 $\vec v$，结合[向量时间导数公式](#3.1. 不同参考系间的时间导数)可得 $$ \vec v = \vec\omega\times\left(\vec p-\overrightarrow{p_0}\right) \tag{4.2} $$ 则结合叉乘求导法则可得目标加速度 $\vec a$ 为 $$ \begin{align} \vec a = \dot{\vec v} &= \dot{\vec\omega}\times\left(\vec p-\overrightarrow{p_0}\right)+\vec\omega\times\dot{\vec p}-\vec\omega\times\dot{\overrightarrow{p_0}} \notag \\ &= \vec\omega\times\dot{\vec p} = \vec\omega\times\vec v \tag{4.3} \end{align} $$ 结合[向量三重积](#2.2 向量三重积属性)可进一步得到3 $$ \begin{align} \vec v\times\vec a &= \vec v\times\left(\vec\omega\times\vec v\right) \notag \\ &= \vec \omega\left(\vec v\cdot\vec v\right)-\vec v\left(\vec v\cdot\vec\omega\right) \notag \\ &= \vec \omega\left(\vec v\cdot\vec v\right) \notag \end{align} $$ 进而有 $$ \vec \omega = \frac{\vec v\times\vec a}{\vec v\cdot\vec v} \tag{4.4} $$ 结合加速度 $\vec a$ 及其[时间导数](#3.1. 不同参考系间的时间导数)则有加速度变化率 $\dot{\vec a}$ 为 $$ \begin{align} \dot{\vec a} &= \dot{\vec\omega}\times\vec v+\vec\omega\times\dot{\vec v} \notag \\ &= \vec\omega\times\left(\vec\omega\times\vec v\right) \notag \\ &= \vec\omega\left(\vec\omega\cdot\vec v\right)-\vec v\left(\vec\omega\cdot\vec\omega\right) = -\omega^2\vec v \tag{4.5} \end{align} $$ 结合 $\dot{\vec p}$ 、$\dot{\vec v}$ 、$\dot{\vec a}$，可得独立分量 $\vec x\mathrel{\vcenter{:}}= \left[\begin{array}{ccc}p_x & \dot{y_x} & \ddot{p_x}\end{array}\right]^T$ 的状态转移矩阵 $$ \dot{\vec x} = \left[\begin{array}{ccc}0 & 1 & 0 \\0 & 0 & 1 \\0 & -\omega^2 & 0\end{array}\right]\vec x \mathrel{\vcenter{:}}= A\cdot {\vec x} \tag{4.6} $$ 则上述微分方程有解，且可以[矩阵指数](#1. 矩阵指数)描述 $$ \vec x = e^{At} $$ 又 $$ e^{At} = \left[\begin{array}{ccc}1 & \frac{\sin(t\omega)}{\omega} & \frac{1-\cos(t\omega)}{\omega^2} \\0 & \cos(t\omega) & \frac{\sin(t\omega)}{\omega} \\0 & -\omega\sin(t\omega) & \cos(t\omega)\end{array}\right] \tag{4.7} $$

PS：此处未完全理顺，待完善

此外，对于CAV模型还存在由角速度 $\omega$ 引起的过程噪声 (process noise)，表示为$V_{\omega}\sim N\left(0, \sigma_{\omega}^2\right)$，其对状态向量的影响可表示为 $$ \begin{equation} \left[ \begin{array}{c} \frac{\omega\Delta t-\sin(\omega\Delta t)}{\omega^3} \\ \frac{1-\cos(\omega\Delta t)}{\omega^2} \\ \frac{\sin(\omega\Delta t)}{\omega} \end{array} \right]V_{\omega} \tag{4.8} \end{equation} $$

5. Constant Turn Rate and Acceleration (CTRA) in 2D

该模型假设目标的运动具备如下约束：

  1. 目标在平面内运动
  2. 目标运动的转向速率不变
  3. 目标运动的加速度不变

基于上述假设，。。。

选择状态向量 $\left[x\ y\ v\ \dot v\ \theta\ \dot{\theta}\right]^T$

定义模型的状态向量为$\overrightarrow{X}$，分别由位置 $x$、位置 $y$、切向速率 $v$、切向加速度 $\dot{v}$、横摆角 $\theta$、横摆角速率 $\dot{\theta}$组成。 $$ \begin{equation} \overrightarrow{X} = \left[ \begin{array}{ccccc} x & y & v & \dot{v} & \theta & \dot{\theta} \end{array} \right]^T \tag{5.1}\\ \end{equation} $$ 其一阶导$\dot{\overrightarrow{X}}$为 $$ \begin{equation} \dot{\overrightarrow{X}} = \left[ \begin{array}{c} \dot{x} = v\cdot cos\theta \\ \dot{y} = v\cdot sin\theta \\ \dot{v} \\ \ddot{v} = 0 \\ \dot{\theta} \\ \ddot{\theta} = 0 \end{array} \right] \tag{5.2} \end{equation} $$ 对$\dot{\overrightarrow{X}}$积分（令 $t_k-t_{k-1}=\Delta t$），可得状态转移方程$\left(\dot{\theta}{t{k-1}} \equiv \dot{\theta} \neq 0\right)$如下 $$ \begin{align} \overrightarrow{X_{t_k}} &= \overrightarrow{X_{t_{k-1}}} + \left[ \begin{array}{c} \int_{t_{k-1}}^{t_k}\left(v\cdot cos\theta\right)dt = \int_{t_{k-1}}^{t_k}\left((v_{t_{k-1}}+\dot{v}_{t_{k-1}}\Delta t)\cdot cos(\theta_{t_{k-1}}+\dot{\theta}_{t_{k-1}}\Delta t)\right)dt \\ \int_{t_{k-1}}^{t_k}\left(v\cdot sin\theta\right)dt = \int_{t_{k-1}}^{t_k}\left((v_{t_{k-1}}+\dot{v}_{t_{k-1}}\Delta t)\cdot sin(\theta_{t_{k-1}}+\dot{\theta}_{t_{k-1}}\Delta t)\right)dt \\ \int_{t_{k-1}}^{t_k}\dot{v}dt = \dot{v}_{t_{k-1}}\Delta t \\ 0 \\ \int_{t_{k-1}}^{t_k}\dot{\theta}dt = \dot{\theta}_{t_{k-1}}\Delta t \\ 0 \end{array} \right] \notag \\ &= \overrightarrow{X_{t_{k-1}}} + \left[ \begin{array}{c} \int_{t_{k-1}}^{t_k}\left(v_{t_{k-1}}\cdot cos(\theta_{t_{k-1}}+\dot{\theta}\Delta t)\right)dt+\int_{t_{k-1}}^{t_k}\left(\dot{v}\Delta t\cdot cos(\theta_{t_{k-1}}+\dot{\theta}\Delta t)\right)dt \\ \int_{t_{k-1}}^{t_k}\left(v_{t_{k-1}}\cdot sin(\theta_{t_{k-1}}+\dot{\theta}\Delta t)\right)dt+\int_{t_{k-1}}^{t_k}\left(\dot{v}\Delta t\cdot sin(\theta_{t_{k-1}}+\dot{\theta}\Delta t)\right)dt \\ \dot{v}\Delta t \\ 0 \\ \dot{\theta}\Delta t \\ 0 \end{array} \right] \notag \\ &= \overrightarrow{X_{t_{k-1}}} + \left[ \begin{array}{c} \frac{v_{t_{k-1}}}{\dot{\theta}}\left(\sin(\dot{\theta}\Delta t+\theta_{t_{k-1}})-\sin(\theta_{t_{k-1}})\right)+\dot{v}\int_{0}^{\Delta t}\left(\Delta t\cdot cos(\theta_{t_{k-1}}+\dot{\theta}\Delta t)\right)d\Delta t \\ \frac{v_{t_{k-1}}}{\dot{\theta}}\left(-\cos(\dot{\theta}\Delta t+\theta_{t_{k-1}})+\cos(\theta_{t_{k-1}})\right)+\dot{v}\int_{0}^{\Delta t}\left(\Delta t\cdot sin(\theta_{t_{k-1}}+\dot{\theta}\Delta t)\right)d\Delta t \\ \dot{v}\Delta t \\ 0 \\ \dot{\theta}\Delta t \\ 0 \end{array} \right] \notag \\ &= \overrightarrow{X_{t_{k-1}}} + \left[ \begin{array}{c} \frac{v_{t_{k-1}}}{\dot{\theta}}\left(\sin(\dot{\theta}\Delta t+\theta_{t_{k-1}})-\sin(\theta_{t_{k-1}})\right)+\dot{v}\left[\frac{\Delta t \cdot sin(\theta_{t_{k-1}}+\dot{\theta}\Delta t)}{\dot{\theta}}+\frac{cos(\theta_{t_{k-1}}+\dot{\theta}\Delta t)}{\dot{\theta}^2}\right]|_0^{\Delta t} \\ \frac{v_{t_{k-1}}}{\dot{\theta}}\left(-\cos(\dot{\theta}\Delta t+\theta_{t_{k-1}})+\cos(\theta_{t_{k-1}})\right)+\dot{v}\left[-\frac{\Delta t \cdot cos(\theta_{t_{k-1}}+\dot{\theta}\Delta t)}{\dot{\theta}}+\frac{sin(\theta_{t_{k-1}}+\dot{\theta}\Delta t)}{\dot{\theta}^2}\right]|_0^{\Delta t} \\ \dot{v}\Delta t \\ 0 \\ \dot{\theta}\Delta t \\ 0 \end{array} \right] \notag \\ &= \overrightarrow{X_{t_{k-1}}} + \left[ \begin{array}{c} \frac{1}{\dot{\theta}}\left(v_{t_{k-1}}+\dot{v}\Delta t\right)sin(\theta_{t_{k-1}}+\dot{\theta}\Delta t)+\frac{\dot{v}}{\dot{\theta}^2}cos(\theta_{t_{k-1}}+\dot{\theta}\Delta t)-\frac{v_{t_{k-1}}}{\dot{\theta}}sin\theta_{t_{k-1}}-\frac{\dot{v}}{\dot{\theta}^2}cos\theta_{t_{k-1}} \\ -\frac{1}{\dot{\theta}}\left(v_{t_{k-1}}+\dot{v}\Delta t\right)cos(\theta_{t_{k-1}}+\dot{\theta}\Delta t)+\frac{\dot{v}}{\dot{\theta}^2}sin(\theta_{t_{k-1}}+\dot{\theta}\Delta t)+\frac{v_{t_{k-1}}}{\dot{\theta}}cos\theta_{t_{k-1}}-\frac{\dot{v}}{\dot{\theta}^2}sin\theta_{t_{k-1}} \\ \dot{v}\Delta t \\ 0 \\ \dot{\theta}\Delta t \\ 0 \end{array} \right] \tag{5.3} \end{align} $$ 在 $\left(\dot{\theta}_{t_{k-1}}=0\right)$ 时状态转移方程如下 $$ \begin{equation} \overrightarrow{X_{t_k}} = \overrightarrow{X_{t_{k-1}}} + \left[ \begin{array}{c} \left(v_{t_{k-1}}\Delta t+\frac{1}{2}\dot{v}\Delta t^2\right)\cos\theta_{t_{k-1}} \\ \left(v_{t_{k-1}}\Delta t+\frac{1}{2}\dot{v}\Delta t^2\right)\sin\theta_{t_{k-1}} \\ \dot{v}\Delta t \\ 0 \\ \dot{\theta}_{t_{k-1}}\Delta t = 0 \\ 0 \end{array} \right] \tag{5.4} \end{equation} $$

6. Constant Steering Angle and Velocity (CSAV) in 2D

该模型假设目标的运动具备如下约束：

  1. 目标在平面内运动
  2. 目标运动的转向角不变
  3. 目标运动的速度不变

基于上述假设，。。。

7. Constant Steering Angle and Acceleration (CSAA) in 2D

该模型假设目标的运动具备如下约束：

  1. 目标在平面内运动
  2. 目标运动的转向角不变
  3. 目标运动的加速度不变

基于上述假设，。。。

8. Constant Curvature and Acceleration (CCA) in 2D / (Aptiv) Coordinated Turn Constant Acceleration (CTCA) in 2D

该模型假设目标的运动具备如下约束：

  1. 目标在平面内运动
  2. 目标运动的轨迹曲率不变
  3. 目标运动的加速度不变

基于上述假设，可以将目标在极小时间片内的运动近似为匀加速圆周运动。

简化计算补充假设：

  1. 极小时间片内，来自模型的过程噪声为常量
  2. 三角函数小角度近似
选择状态向量 $\left[x\ y\ v\ \dot{v}\ \psi\ c\right]^T$

定义模型的状态向量为 $\overrightarrow{X}$，分别由位置 $x$、位置 $y$、切向速率 $v$、切向加速度 $\dot{v}$ 、横摆角 $\psi$、轨迹曲率 $c$组成。 $$ \begin{equation} \overrightarrow{X} = \left[ \begin{array}{ccccc} x & y & v & \dot{v} & \psi & c \end{array} \right]^T \tag{8.1}\\ \end{equation} $$ 其一阶导$\dot{\overrightarrow{X}}$为 $$ \begin{equation} \dot{\overrightarrow{X}} = \left[ \begin{array}{c} \dot{x} = v\cdot cos\psi \\ \dot{y} = v\cdot sin\psi \\ \dot{v} \\ \ddot{v} = 0 \\ \dot{\psi} = c\cdot v \\ \dot{c} = 0 \end{array} \right] \tag{8.2} \end{equation} $$ 若模型存在一定的不确定性，即存在由 $\ddot{v}$ 和 $\dot{c}$ 引起的过程噪声 $V_{\ddot v}$ 和 $V_{\dot c}$，且满足 $V_{\ddot{v}}\sim N\left(0, \sigma_{\ddot{v}}^2\right)$ 和 $V_{\dot{c}}\sim N\left(0, \sigma_{\dot{c}}^2\right)$，则状态向量的一阶导可修正为 $$ \begin{equation} \dot{\overrightarrow{X}} = \left[ \begin{array}{c} v\cdot cos\psi \\ v\cdot sin\psi \\ \dot{v} \\ 0 \\ c\cdot v \\ 0 \end{array} \right] + \left[ \begin{array}{c} 0 \\ 0 \\ 0 \\ V_{\ddot v}\mathrel{\vcenter{:}}=\eta \\ 0 \\ V_{\dot c}\mathrel{\vcenter{:}}=\zeta \end{array} \right]\tag{8.3} \end{equation} $$ 可将上述关于状态量的微分方程组拆分为线性部分和非线性部分，则线性部分为 $$ \begin{align} \dot{\overrightarrow{X}}_{linear} &= \left[ \begin{array}{c} \dot{v} \\ 0 \\ 0 \end{array} \right] + \left[ \begin{array}{c} 0 \\ V_{\ddot v}\mathrel{\vcenter{:}}=\eta \\ V_{\dot c}\mathrel{\vcenter{:}}=\zeta \end{array} \right] \notag \\ &= \left[\begin{array}{ccc}0 & 1 & 0 \\0 & 0 & 0 \\0 & 0 & 0\end{array}\right]\overrightarrow{X}_{linear}+ \left[\begin{array}{cc}0 & 0 \\1 & 0 \\0 & 1\end{array}\right]\left[\begin{array}{c} \eta \\\zeta\end{array}\right] \tag{8.4} \end{align} $$ 则该一阶常系数非齐次线性微分方程在时间区间 $[t_0,t)$ 上的解为 $$ \begin{align} \overrightarrow{X}_{linear} &= e^{\left(\left[\begin{array}{ccc}0 & 1 & 0 \\0 & 0 & 0 \\0 & 0 & 0\end{array}\right](t-t_0)\right)}\cdot\overrightarrow{X}_{linear}(t_0)+\int^t_{t_0}e^{\left(\left[\begin{array}{ccc}0 & 1 & 0 \\0 & 0 & 0 \\0 & 0 & 0\end{array}\right](t-\tau)\right)}\left[\begin{array}{cc}0 & 0 \\1 & 0 \\0 & 1\end{array}\right]\left[\begin{array}{c} \eta(\tau) \\\zeta(\tau)\end{array}\right]d\tau \notag \\ &= \left[\begin{array}{ccc}1 & t-t_0 & 0 \\0 & 1 & 0 \\0 & 0 & 1\end{array}\right]\cdot\overrightarrow{X}_{linear}(t_0)+ \int^t_{t_0}\left[\begin{array}{c}\eta(\tau)\cdot(t-\tau) \\\eta(\tau) \\\zeta(\tau)\end{array}\right]d\tau \notag \\ &= \left[\begin{array}{c}v(t_0)+\dot{v}(t_0)\cdot(t-t_0) \\\dot{v}(t_0) \\c(t_0)\end{array}\right]+ \left[\begin{array}{c}\eta(t_0)\cdot\frac{(t-t_0)^2}{2} \\\eta(t_0)\cdot(t-t_0) \\\zeta(t_0)\cdot(t-t_0)\end{array}\right] \tag{8.5} \end{align} $$ 其中根据简化计算假设在时间区间 $[t_0,t)$ 上模型的过程噪声为常量，即 $\eta(\tau)$ 和 $\zeta(\tau)$ 是常量。

提示：

矩阵指数的计算可以借助用于符号计算的开源 Python 库 SymPy，如

import sympy
from sympy import init_printing
init_printing(use_latex=True)

t, t0 = sympy.symbols("t t_0")
A = sympy.Matrix([[0, 0, t-t0], [0, 0, 0], [0, 0, 0]])
eA = sympy.simplify(sympy.exp(A))
print(f"The exponential of A is: \n\n{sympy.pretty(eA)}")

再逐步计算非线性部分，首先求解 $\frac{\mathrm{d}\psi}{\mathrm{d}t}=c(t)\cdot v(t)$，带入式 $(8.5)$ 的结果有 $$ \begin{align} \frac{\mathrm{d}\psi}{\mathrm{d}t} &= c(t)\cdot v(t) \notag \\ &= \left(c(t_0)+\zeta(t_0)\cdot(t-t_0)\right)\cdot\left(v(t_0)+\dot{v}(t_0)\cdot(t-t_0)+\eta(t_0)\cdot\frac{(t-t_0)^2}{2}\right) \tag{8.6} \end{align} $$ 对其积分，且记 $\Delta t \mathrel{\vcenter{:}}= t-t_0$，则有 $$ \begin{align} \psi(t) &= \psi(t_0) + \int^t_{t_0}\left(c(t_0)+\zeta(t_0)\cdot(\tau-t_0)\right)\cdot\left(v(t_0)+\dot{v}(t_0)\cdot(\tau-t_0)+\eta(t_0)\cdot\frac{(\tau-t_0)^2}{2}\right)\mathrm{d}\tau \notag \\ &= \psi(t_0) + c(t_0)v(t_0)\Delta t+\zeta(t_0)v(t_0)\frac{(\Delta t)^2}{2} + c(t_0)\dot{v}(t_0)\frac{(\Delta t)^2}{2}+\zeta(t_0)\dot{v}(t_0)\frac{(\Delta t)^3}{3} + \eta(t_0)c(t_0)\frac{(\Delta t)^3}{6}+\eta(t_0)\zeta(t_0)\frac{(\Delta t)^4}{8} \notag \\ &= \psi(t_0) + c(t_0)v(t_0)\Delta t + c(t_0)\dot{v}(t_0)\frac{(\Delta t)^2}{2} + \zeta(t_0)\left(v(t_0)\frac{(\Delta t)^2}{2}+\dot{v}(t_0)\frac{(\Delta t)^3}{3}\right) + \eta(t_0)c(t_0)\frac{(\Delta t)^3}{6}+\eta(t_0)\zeta(t_0)\frac{(\Delta t)^4}{8} \tag{8.7} \\ &\mathrel{\vcenter{:}}= \psi(t_0) + c(t_0)v(t_0)\Delta t + c(t_0)\dot{v}(t_0)\frac{(\Delta t)^2}{2} + \zeta(t_0)\alpha_1+\eta(t_0)\alpha_2+\eta(t_0)\zeta(t_0)\alpha_3 \tag{8.8} \end{align} $$ 其次求解 $\frac{\mathrm{d}x}{\mathrm{d}t}=v(t)\cdot\mathrm{cos}(\psi(t))$，带入式 $(8.5, 8.8)$ 的结果有 $$ \begin{align} \frac{\mathrm{d}x}{\mathrm{d}t} &= v(t)\cdot\mathrm{cos}(\psi(t)) \notag \\ &= \left(v(t_0)+\dot{v}(t_0)\cdot(t-t_0)+\eta(t_0)\cdot\frac{(t-t_0)^2}{2}\right) \cdot\mathrm{cos}\left(\psi(t_0)+c(t_0)v(t_0)\Delta t+c(t_0)\dot{v}(t_0)\frac{(\Delta t)^2}{2} +\zeta(t_0)\alpha_1+\eta(t_0)\alpha_2+\eta(t_0)\zeta(t_0)\alpha_3\right) \tag{8.9} \end{align} $$ 根据简化计算假设，如果 $\Delta \theta$ 足够小，则 $(0.1),\ (0.2)$ 成立。记 $\Delta t \mathrel{\vcenter{:}}= t-t_0$，若其足够小，则其高阶项将更小。对 $(8.9)$ 进行积分，则有 $$ \begin{array}{l} x(t) &=& x(t_0) + \int^t_{t_0}\left(v(t_0)+\dot{v}(t_0)\cdot\Delta t+\eta(t_0)\cdot\frac{(\Delta t)^2}{2}\right) \cdot\mathrm{cos}\left(\psi(t_0)+c(t_0)v(t_0)\Delta t+c(t_0)\dot{v}(t_0)\frac{(\Delta t)^2}{2} +\zeta(t_0)\alpha_1+\eta(t_0)\alpha_2+\eta(t_0)\zeta(t_0)\alpha_3\right)\mathrm{d}t \\ &\mathrel{\vcenter{:}}=& x(t_0) + \int^t_{t_0}\left(v(t_0)+\dot{v}(t_0)\cdot\Delta t+\eta(t_0)\cdot\frac{(\Delta t)^2}{2}\right) \cdot\mathrm{cos}\left(\psi(t_0)+\beta(t)\right)\mathrm{d}t \\ &=& x(t_0) + \int^t_{t_0}\left(v(t_0)+\dot{v}(t_0)\cdot\Delta t+\eta(t_0)\cdot\frac{(\Delta t)^2}{2}\right) \cdot\left(\mathrm{cos}(\psi(t_0))-\beta(t)\mathrm{sin}(\psi(t_0))\right)\mathrm{d}t \\ &=& x(t_0) + v(t_0)\mathrm{cos}(\psi(t_0))\Delta t + \dot{v}(t_0)\mathrm{cos}(\psi(t_0))\frac{(\Delta t)^2}{2} + \eta(t_0)\mathrm{cos}(\psi(t_0))\frac{(\Delta t)^3}{6} \\ &&- \left(v(t_0)\mathrm{sin}(\psi(t_0))\int^t_{t_0}\beta(t)\mathrm{d}t + \dot{v}(t_0)\mathrm{sin}(\psi(t_0))\int^t_{t_0}\Delta t\beta(t)\mathrm{d}t + \eta(t_0)\mathrm{sin}(\psi(t_0))\int^t_{t_0}\frac{(\Delta t)^2}{2}\beta(t)\mathrm{d}t\right) \\ &=& x(t_0) + v(t_0)\mathrm{cos}(\psi(t_0))\Delta t + \dot{v}(t_0)\mathrm{cos}(\psi(t_0))\frac{(\Delta t)^2}{2} + \eta(t_0)\mathrm{cos}(\psi(t_0))\frac{(\Delta t)^3}{6} \\ &&- v(t_0)\mathrm{sin}(\psi(t_0))\left(c(t_0)v(t_0)\frac{(\Delta t)^2}{2} + c(t_0)\dot{v}(t_0)\frac{(\Delta t)^3}{6} + \zeta(t_0)\left(v(t_0)\frac{(\Delta t)^3}{6}+\dot{v}(t_0)\frac{(\Delta t)^4}{12}\right) + \eta(t_0)c(t_0)\frac{(\Delta t)^4}{24} + \eta(t_0)\zeta(t_0)\frac{(\Delta t)^5}{40}\right) \\ &&- \dot{v}(t_0)\mathrm{sin}(\psi(t_0))\left(c(t_0)v(t_0)\frac{(\Delta t)^3}{3} + c(t_0)\dot{v}(t_0)\frac{(\Delta t)^4}{8} + \zeta(t_0)\left(v(t_0)\frac{(\Delta t)^4}{8}+\dot{v}(t_0)\frac{(\Delta t)^5}{15}\right) + \eta(t_0)c(t_0)\frac{(\Delta t)^5}{30} + \eta(t_0)\zeta(t_0)\frac{(\Delta t)^6}{48}\right) \\ &&- \eta(t_0)\mathrm{sin}(\psi(t_0))\left(c(t_0)v(t_0)\frac{(\Delta t)^4}{8} + c(t_0)\dot{v}(t_0)\frac{(\Delta t)^5}{20} + \zeta(t_0)\left(v(t_0)\frac{(\Delta t)^5}{20}+\dot{v}(t_0)\frac{(\Delta t)^6}{36}\right) + \eta(t_0)c(t_0)\frac{(\Delta t)^6}{72} + \eta(t_0)\zeta(t_0)\frac{(\Delta t)^7}{112}\right) \\ &=& x(t_0) + \mathrm{cos}(\psi(t_0))\left(v(t_0)\Delta t+\dot{v}(t_0)\frac{(\Delta t)^2}{2}\right) - c(t_0)\mathrm{sin}(\psi(t_0))\left(v^2(t_0)\frac{(\Delta t)^2}{2} + v(t_0)\dot{v}(t_0)\frac{(\Delta t)^3}{2} + \dot{v}^2(t_0)\frac{(\Delta t)^4}{8}\right) \\ &&- \zeta(t_0)\cdot\mathrm{sin}(\psi(t_0))\cdot\left(v^2(t_0)\frac{(\Delta t)^3}{6} + v(t_0)\dot{v}(t_0)\frac{5(\Delta t)^4}{24} + \dot{v}^2(t_0)\frac{(\Delta t)^5}{15}\right) \\ &&+ \eta(t_0)\cdot\left(\mathrm{cos}(\psi(t_0))\frac{(\Delta t)^3}{6} - \mathrm{sin}(\psi(t_0))\cdot c(t_0)\left(v_{t_0}\frac{(\Delta t)^4}{6} + \dot{v}_{t_0}\frac{(\Delta t)^5}{12}\right)\right) \\ &&- \eta(t_0)\zeta(t_0)\cdot\mathrm{sin}(\psi(t_0))\cdot\left(v(t_0)\frac{3(\Delta t)^5}{40} + \dot{v}(t_0)\frac{7(\Delta t)^6}{144}\right) \\ &&- \eta^2(t_0)\cdot\mathrm{sin}(\psi(t_0))\cdot c(t_0)\frac{(\Delta t)^6}{72} \\ &&- \zeta(t_0)\eta^2(t_0)\cdot\mathrm{sin}(\psi(t_0))\cdot\frac{(\Delta t)^7}{112} \tag{8.10} \end{array} $$ 最后求解 $\frac{\mathrm{d}y}{\mathrm{d}t}=v(t)\cdot\mathrm{sin}(\psi(t))$，带入式 $(8.5, 8.8)$ 的结果有 $$ \begin{align} \frac{\mathrm{d}y}{\mathrm{d}t} &= v(t)\cdot\mathrm{sin}(\psi(t)) \notag \\ &= \left(v(t_0)+\dot{v}(t_0)\cdot(t-t_0)+\eta(t_0)\cdot\frac{(t-t_0)^2}{2}\right) \cdot\mathrm{sin}\left(\psi(t_0)+c(t_0)v(t_0)\Delta t+c(t_0)\dot{v}(t_0)\frac{(\Delta t)^2}{2} +\zeta(t_0)\alpha_1+\eta(t_0)\alpha_2+\eta(t_0)\zeta(t_0)\alpha_3\right) \tag{8.11} \end{align} $$ 同样根据简化计算假设，如果 $\Delta \theta$ 足够小，则 $(0.1),\ (0.2)$ 成立。记 $\Delta t \mathrel{\vcenter{:}}= t-t_0$，若其足够小，则其高阶项将更小。对 $(8.11)$ 进行积分，则有 $$ \begin{array}{l} y(t) &=& y(t_0) + \int^t_{t_0}\left(v(t_0)+\dot{v}(t_0)\cdot\Delta t+\eta(t_0)\cdot\frac{(\Delta t)^2}{2}\right) \cdot\mathrm{sin}\left(\psi(t_0)+c(t_0)v(t_0)\Delta t+c(t_0)\dot{v}(t_0)\frac{(\Delta t)^2}{2} +\zeta(t_0)\alpha_1+\eta(t_0)\alpha_2+\eta(t_0)\zeta(t_0)\alpha_3\right)\mathrm{d}t \\ &\mathrel{\vcenter{:}}=& y(t_0) + \int^t_{t_0}\left(v(t_0)+\dot{v}(t_0)\cdot\Delta t+\eta(t_0)\cdot\frac{(\Delta t)^2}{2}\right) \cdot\mathrm{sin}\left(\psi(t_0)+\beta(t)\right)\mathrm{d}t \\ &=& y(t_0) + \int^t_{t_0}\left(v(t_0)+\dot{v}(t_0)\cdot\Delta t+\eta(t_0)\cdot\frac{(\Delta t)^2}{2}\right) \cdot\left(\mathrm{sin}(\psi(t_0))+\beta(t)\mathrm{cos}(\psi(t_0))\right)\mathrm{d}t \\ &=& y(t_0) + v(t_0)\mathrm{sin}(\psi(t_0))\Delta t + \dot{v}(t_0)\mathrm{sin}(\psi(t_0))\frac{(\Delta t)^2}{2} + \eta(t_0)\mathrm{sin}(\psi(t_0))\frac{(\Delta t)^3}{6} \\ &&+ \left(v(t_0)\mathrm{cos}(\psi(t_0))\int^t_{t_0}\beta(t)\mathrm{d}t + \dot{v}(t_0)\mathrm{cos}(\psi(t_0))\int^t_{t_0}\Delta t\beta(t)\mathrm{d}t + \eta(t_0)\mathrm{cos}(\psi(t_0))\int^t_{t_0}\frac{(\Delta t)^2}{2}\beta(t)\mathrm{d}t\right) \\ &=& y(t_0) + v(t_0)\mathrm{sin}(\psi(t_0))\Delta t + \dot{v}(t_0)\mathrm{sin}(\psi(t_0))\frac{(\Delta t)^2}{2} + \eta(t_0)\mathrm{sin}(\psi(t_0))\frac{(\Delta t)^3}{6} \\ &&+ v(t_0)\mathrm{cos}(\psi(t_0))\left(c(t_0)v(t_0)\frac{(\Delta t)^2}{2} + c(t_0)\dot{v}(t_0)\frac{(\Delta t)^3}{6} + \zeta(t_0)\left(v(t_0)\frac{(\Delta t)^3}{6}+\dot{v}(t_0)\frac{(\Delta t)^4}{12}\right) + \eta(t_0)c(t_0)\frac{(\Delta t)^4}{24} + \eta(t_0)\zeta(t_0)\frac{(\Delta t)^5}{40}\right) \\ &&+ \dot{v}(t_0)\mathrm{cos}(\psi(t_0))\left(c(t_0)v(t_0)\frac{(\Delta t)^3}{3} + c(t_0)\dot{v}(t_0)\frac{(\Delta t)^4}{8} + \zeta(t_0)\left(v(t_0)\frac{(\Delta t)^4}{8}+\dot{v}(t_0)\frac{(\Delta t)^5}{15}\right) + \eta(t_0)c(t_0)\frac{(\Delta t)^5}{30} + \eta(t_0)\zeta(t_0)\frac{(\Delta t)^6}{48}\right) \\ &&+ \eta(t_0)\mathrm{cos}(\psi(t_0))\left(c(t_0)v(t_0)\frac{(\Delta t)^4}{8} + c(t_0)\dot{v}(t_0)\frac{(\Delta t)^5}{20} + \zeta(t_0)\left(v(t_0)\frac{(\Delta t)^5}{20}+\dot{v}(t_0)\frac{(\Delta t)^6}{36}\right) + \eta(t_0)c(t_0)\frac{(\Delta t)^6}{72} + \eta(t_0)\zeta(t_0)\frac{(\Delta t)^7}{112}\right) \\ &=& y(t_0) + \mathrm{sin}(\psi(t_0))\left(v(t_0)\Delta t+\dot{v}(t_0)\frac{(\Delta t)^2}{2}\right) + c(t_0)\mathrm{cos}(\psi(t_0))\left(v^2(t_0)\frac{(\Delta t)^2}{2} + v(t_0)\dot{v}(t_0)\frac{(\Delta t)^3}{2} + \dot{v}^2(t_0)\frac{(\Delta t)^4}{8}\right) \\ &&+ \zeta(t_0)\cdot\mathrm{cos}(\psi(t_0))\cdot\left(v^2(t_0)\frac{(\Delta t)^3}{6} + v(t_0)\dot{v}(t_0)\frac{5(\Delta t)^4}{24} + \dot{v}^2(t_0)\frac{(\Delta t)^5}{15}\right) \\ &&+ \eta(t_0)\cdot\left(\mathrm{sin}(\psi(t_0))\frac{(\Delta t)^3}{6} + \mathrm{cos}(\psi(t_0))\cdot c(t_0)\left(v_{t_0}\frac{(\Delta t)^4}{6} + \dot{v}_{t_0}\frac{(\Delta t)^5}{12}\right)\right) \\ &&+ \eta(t_0)\zeta(t_0)\cdot\mathrm{cos}(\psi(t_0))\cdot\left(v(t_0)\frac{3(\Delta t)^5}{40} + \dot{v}(t_0)\frac{7(\Delta t)^6}{144}\right) \\ &&+ \eta^2(t_0)\cdot\mathrm{cos}(\psi(t_0))\cdot c(t_0)\frac{(\Delta t)^6}{72} \\ &&+ \zeta(t_0)\eta^2(t_0)\cdot\mathrm{cos}(\psi(t_0))\cdot\frac{(\Delta t)^7}{112} \tag{8.12} \end{array} $$ 为便于书写及表达，做如下简记： $$ \begin{align} \lambda_1 &= \mathrm{sin}(\psi(t_0))\cdot\left(v^2(t_0)\frac{(\Delta t)^3}{6} + v(t_0)\dot{v}(t_0)\frac{5(\Delta t)^4}{24} + \dot{v}^2(t_0)\frac{(\Delta t)^5}{15}\right) \tag{8.13} \\ \lambda_2 &= \mathrm{cos}(\psi(t_0))\frac{(\Delta t)^3}{6} - \mathrm{sin}(\psi(t_0))\cdot c(t_0)\left(v_{t_0}\frac{(\Delta t)^4}{6} + \dot{v}_{t_0}\frac{(\Delta t)^5}{12}\right) \tag{8.14} \\ \lambda_3 &= \mathrm{sin}(\psi(t_0))\cdot\left(v(t_0)\frac{3(\Delta t)^5}{40} + \dot{v}(t_0)\frac{7(\Delta t)^6}{144}\right) \tag{8.15} \\ \lambda_4 &= \mathrm{sin}(\psi(t_0))\cdot c(t_0)\frac{(\Delta t)^6}{72} \tag{8.16} \\ \lambda_5 &= \mathrm{sin}(\psi(t_0))\cdot\frac{(\Delta t)^7}{112} \tag{8.17} \\ \lambda_6 &= \mathrm{cos}(\psi(t_0))\cdot\left(v^2(t_0)\frac{(\Delta t)^3}{6} + v(t_0)\dot{v}(t_0)\frac{5(\Delta t)^4}{24} + \dot{v}^2(t_0)\frac{(\Delta t)^5}{15}\right) \tag{8.18} \\ \lambda_7 &= \mathrm{sin}(\psi(t_0))\frac{(\Delta t)^3}{6} + \mathrm{cos}(\psi(t_0))\cdot c(t_0)\left(v_{t_0}\frac{(\Delta t)^4}{6} + \dot{v}_{t_0}\frac{(\Delta t)^5}{12}\right) \tag{8.19} \\ \lambda_8 &= \mathrm{cos}(\psi(t_0))\cdot\left(v(t_0)\frac{3(\Delta t)^5}{40} + \dot{v}(t_0)\frac{7(\Delta t)^6}{144}\right) \tag{8.20} \\ \lambda_9 &= \mathrm{cos}(\psi(t_0))\cdot c(t_0)\frac{(\Delta t)^6}{72} \tag{8.21} \\ \lambda_{10} &= \mathrm{cos}(\psi(t_0))\cdot\frac{(\Delta t)^7}{112} \tag{8.22} \\ \lambda_{11} &= \frac{(\Delta t)^2}{2} \tag{8.23} \\ \lambda_{12} &= \Delta t \tag{8.24} \\ \lambda_{13} &= v(t_0)\frac{(\Delta t)^2}{2}+\dot{v}(t_0)\frac{(\Delta t)^3}{3} \tag{8.25} \\ \lambda_{14} &= c(t_0)\frac{(\Delta t)^3}{6} \tag{8.26} \\ \lambda_{15} &= \frac{(\Delta t)^4}{8} \tag{8.27} \end{align} $$ 因此，状态向量的递推式可表述如下 $$ \begin{array}{l} \overrightarrow{X_{t_k}} =& \overrightarrow{X_{t_{k-1}}} &+ \left[ \begin{array}{c} \mathrm{cos}(\psi(t_{k-1}))\left(v(t_{k-1})\Delta t+\dot{v}(t_{k-1})\frac{(\Delta t)^2}{2}\right) - c(t_{k-1})\mathrm{sin}(\psi(t_{k-1}))\left(v^2(t_{k-1})\frac{(\Delta t)^2}{2} + v(t_{k-1})\dot{v}(t_{k-1})\frac{(\Delta t)^3}{2} + \dot{v}^2(t_{k-1})\frac{(\Delta t)^4}{8}\right) \\ \mathrm{sin}(\psi(t_{k-1}))\left(v(t_{k-1})\Delta t+\dot{v}(t_{k-1})\frac{(\Delta t)^2}{2}\right) + c(t_{k-1})\mathrm{cos}(\psi(t_{k-1}))\left(v^2(t_{k-1})\frac{(\Delta t)^2}{2} + v(t_{k-1})\dot{v}(t_{k-1})\frac{(\Delta t)^3}{2} + \dot{v}^2(t_{k-1})\frac{(\Delta t)^4}{8}\right) \\ \dot{v}(t_{k-1})\Delta t \\ 0 \\ c(t_{k-1})v(t_{k-1})\Delta t + c(t_{k-1})\dot{v}(t_{k-1})\frac{(\Delta t)^2}{2} \\ 0 \end{array} \right] \\ &&+ \left[ \begin{array}{c} -\zeta(t_{k-1})\lambda_1+\eta(t_{k-1})\lambda_2-\eta(t_{k-1})\zeta(t_{k-1})\lambda_3-\eta^2(t_{k-1})\lambda_4-\zeta(t_{k-1})\eta^2(t_{k-1})\lambda_5 \\ \zeta(t_{k-1})\lambda_6+\eta(t_{k-1})\lambda_7+\eta(t_{k-1})\zeta(t_{k-1})\lambda_8+\eta^2(t_{k-1})\lambda_9+\zeta(t_{k-1})\eta^2(t_{k-1})\lambda_{10} \\ \eta(t_{k-1})\lambda_{11} \\ \eta(t_{k-1})\lambda_{12} \\ \zeta(t_{k-1})\lambda_{13}+\eta(t_{k-1})\lambda_{14}+\eta(t_{k-1})\zeta(t_{k-1})\lambda_{15} \\ \zeta(t_{k-1})\lambda_{12} \end{array} \right] \tag{8.28} \end{array} $$

提示：

该状态转移方程 Jacobian 矩阵的计算可以借助用于符号计算的开源 Python 库 SymPy，如

import sympy
from sympy import sin, cos, init_printing
init_printing(use_latex=True)

px, py, vv, aa, psi, curvature = sympy.symbols("x y v a psi c")
px0, py0, vv0, aa0, psi0, curvature0, t = sympy.symbols("x_0 y_0 v_0 a_0 psi_0 c_0 T")

px = px0+cos(psi0)*(vv0*t+aa0*(t**2)/2)-curvature0*sin(psi0)*((vv0**2)*(t**2)/2+vv0*aa0*(t**3)/2+(aa0**2)*(t**4)/8)
py = py0+sin(psi0)*(vv0*t+aa0*(t**2)/2)+curvature0*cos(psi0)*((vv0**2)*(t**2)/2+vv0*aa0*(t**3)/2+(aa0**2)*(t**4)/8)
vv = vv0+aa0*t
aa = aa0
psi = psi0+curvature0*vv0*t+curvature0*aa0*(t**2)/2
curvature = curvature0

funcs = sympy.Matrix([px, py, vv, aa, psi, curvature])
args = sympy.Matrix([px0, py0, vv0, aa0, psi0, curvature0])
result = funcs.jacobian(args)

print(f"The Jacobian of state is: \n\n{sympy.pretty(result)}")

借助 Matlab 同样可以，如

syms x y v a psi c x_0 y_0 v_0 a_0 psi_0 c_0 T

as = [x_0 y_0 v_0 a_0 psi_0 c_0];
fs = [x_0+cos(psi_0)*(v_0*T+a_0*(T^2)/2)-c_0*sin(psi_0)*((v_0^2)*(T^2)/2+v_0*a_0*(T^3)/2+(a_0^2)*(T^4)/8);
      y_0+sin(psi_0)*(v_0*T+a_0*(T^2)/2)+c_0*cos(psi_0)*((v_0^2)*(T^2)/2+v_0*a_0*(T^3)/2+(a_0^2)*(T^4)/8);
      v_0+a_0*T;
      a_0;
      psi_0+c_0*v_0*T+c_0*a_0*(T^2)/2;
      c_0];

res = jacobian(fs, as);
latex(res)

Reference

  1. Udacity UKF Unscented Kalman Filter 笔记
  2. 高级运动模型和扩展卡尔曼滤波
  3. 卡尔曼运动模型公式推导CTRV+CTRA
  4. 无迹卡尔曼滤波 CTRV&&CTRA模型

  1. 积分因子可按导数乘法法则配凑 ↩︎

  2. 积分原函数可通过三角函数配凑法求解 ↩︎

  3. 速度 $\vec v$ 与 角速度 $\vec\omega$ 正交，因此向量积为零 ↩︎

更新于 2023-10-25
 运动学, 保护
返回 | 主页