3维空间目标跟踪的CV,CA,CT动力学模型

类似于二维平面上车辆转动的CV，CA，CT模型。3维空间的CV、CA、CT模型也存在。用于目标跟踪时，需要考虑的角速度、速度、加速度之间的耦合关系更为复杂，所以本博客列举了这些公式。

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

本博客突出CT模型，用于空中目标跟踪。

文章目录

质点运动自然坐标系的分解
二维平面运动学总结
三维空间运动模型
- CV
- CA
- 三维恒定速率、角速率（CS-CT）运动
- 三维平面运动CTRV模型
- 三维弹道系分解的CTRA模型
- 平面ACT模型
参考

质点运动自然坐标系的分解

考虑质点运动学偏导数与全导数的关系：
v=drdt=∂r∂t+ω×ra≡dvdt=∂v∂t+ω×v\boldsymbol{v}=\frac{\mathrm{d} \boldsymbol{r}}{\mathrm{d} t}=\frac{\partial \boldsymbol{r}}{\partial t}+\boldsymbol{\omega} \times \boldsymbol{r}\\ \boldsymbol{a}\equiv\frac{\mathrm d \boldsymbol v}{\mathrm d t}= \frac{\partial \boldsymbol v}{\partial t}+\boldsymbol{\omega} \times \boldsymbol v v=dtdr=∂t∂r+ω×ra≡dtdv=∂t∂v+ω×v

一般的三维平面运动，在自然坐标系下的切向和法向的定义为：

速度方向为切向τ\boldsymbol \tauτ
与速度垂直，指向曲线凹陷的一侧为法向n\boldsymbol nn
切向与法向张成密切平面

注意这是一个动坐标系，相对惯性系的速度是v\boldsymbol vv，旋转角速度是w\boldsymbol ww（图中为Ω）。

惯性系下的加速度落在密切平面内，可得加速度分解的惠更斯公式
a≡dvdt=at+an\boldsymbol a\equiv\frac{\mathrm d \boldsymbol v}{\mathrm d t} =\boldsymbol a_t+\boldsymbol a_n a≡dtdv=at+an或者表示为
at=∂v∂t=at⋅τan=ω×v\boldsymbol a_t=\frac{\partial \boldsymbol v}{\partial t}=a_t \cdot\boldsymbol \tau\\ \boldsymbol a_n=\boldsymbol{\omega} \times \boldsymbol v at=∂t∂v=at⋅τan=ω×v上图展示了这种现象，也就是在Span{τ,n}\text{Span}\{\boldsymbol \tau, \boldsymbol n \}Span{τ,n}的正交空间内不存在加速度的分量。切向的单位矢量
τ=[vxv,vyv,vzv]T,v=∥v∥\boldsymbol \tau=\left[\frac{v_x}{v}, \frac{v_y}{v}, \frac{v_z}{v}\right]^{\mathrm T},v=\|\boldsymbol v\| τ=[vvx,vvy,vvz]T,v=∥v∥

二维平面运动学总结

CT模型，或者一般的二维平面运动，都可写作密切平面内的如下形式：
x˙(t)=v(t)cos⁡ϕ(t)y˙(t)=v(t)sin⁡ϕ(t)v˙(t)=at(t)ϕ˙(t)=ar(t)/v(t)≡w(1)\begin{aligned} &\dot{x}(t)=v(t) \cos \phi(t) \\ &\dot{y}(t)=v(t) \sin \phi(t) \\ &\dot{v}(t)=a_{t}(t) \\ &\dot{\phi}(t)=a_{r}(t) / v(t) \equiv w \end{aligned}\tag{1} x˙(t)=v(t)cosϕ(t)y˙(t)=v(t)sinϕ(t)v˙(t)=at(t)ϕ˙(t)=ar(t)/v(t)≡w(1)其中an,ata_n,a_tan,at分别代表法向加速度与切向加速度，www为转弯角速度，ϕ\phiϕ为速度方向角ϕ=arctan⁡(vy/vx)=arcsin⁡(vy/v)\phi=\arctan({v_y}/{v_x})=\arcsin({v_y}/{v})ϕ=arctan(vy/vx)=arcsin(vy/v)。

an=0,at=0a_n=0,a_t=0an=0,at=0——直线，CV运动
an=0,at≠0a_n=0,a_t\neq0an=0,at=0——直线，加速运动。若at=Ca_t=Cat=C，CA运动
an≠0,at=0a_n\neq0,a_t=0an=0,at=0——弧线，匀速率运动。若an=Ca_n=Can=C，则为CTRV运动
an≠0,at≠0a_n\neq0,a_t\neq0an=0,at=0——曲线运动。若an=C1,at=C2a_n=C_1,a_t=C_2an=C1,at=C2，则为CTRA运动

假设CTRV运动的角速度www已知，写到笛卡尔坐标系下，动力学方程成了
x˙(t)=vx(t)y˙(t)=vy(t)v˙x(t)=ax=−w⋅vyv˙y(t)=ay=+w⋅vx(2)\begin{aligned} &\dot{x}(t)=v_x(t) \\ &\dot{y}(t)=v_y(t)\\ &\dot{v}_x(t)=a_x=-w \cdot v_y\\ &\dot{v}_y(t)=a_y=+w \cdot v_x \end{aligned}\tag{2} x˙(t)=vx(t)y˙(t)=vy(t)v˙x(t)=ax=−w⋅vyv˙y(t)=ay=+w⋅vx(2)上式和式(1)的关系是：对于CTRV模型，局部加速度∂v∂t\frac{\partial \boldsymbol v}{\partial t}∂t∂v为0，则惯性系的加速度dvdt=[ax,ay]\frac{\mathrm d\boldsymbol v}{\mathrm dt}=[a_x,a_y]dtdv=[ax,ay]只由旋转造成，即a=ω×v\boldsymbol a=\boldsymbol{\omega} \times \boldsymbol va=ω×v，它的大小a=at2+an2a=\sqrt{a_t^2+a_n^2}a=at2+an2.

式(2)中只有1项最终的不确定量，加速度表示为有关www_www的白噪声，变换可得到。角速度www未知，则写成白噪声或一阶Markov过程。

ω˙(t)=wwω˙(t)=−1τwω(t)+ww\dot \omega(t)=\text{w}_w\\ \dot \omega(t)=-\frac{1}{\tau _w}\omega(t)+\text{w}_w ω˙(t)=wwω˙(t)=−τw1ω(t)+ww上式与公式(1)-(2)可联立，构成5维的状态空间模型，其噪声是www_www. 再将加速度大小合二为一，则CTRA模型是：
x˙(t)=v(t)cos⁡ϕ(t)y˙(t)=v(t)sin⁡ϕ(t)v˙(t)=a(t)ϕ˙(t)=ω(t)ω˙(t)=−1τwω(t)+wwa˙(t)=wa(3)\begin{aligned} &\dot{x}(t)=v(t) \cos \phi(t) \\ &\dot{y}(t)=v(t) \sin \phi(t) \\ &\dot{v}(t)=a(t) \\ &\dot{\phi}(t)=\omega(t)\\ &\dot{\omega}(t)=-\frac{1}{\tau _w}\omega(t)+\text{w}_w\\ &\dot{a}(t)=\text{w}_a \end{aligned}\tag{3} x˙(t)=v(t)cosϕ(t)y˙(t)=v(t)sinϕ(t)v˙(t)=a(t)ϕ˙(t)=ω(t)ω˙(t)=−τw1ω(t)+wwa˙(t)=wa(3)其中ww,wa\text{w}_w,\text{w}_aww,wa代表角速度大小的噪声、加速度大小的噪声；将上式的部分项建模为随机噪声就成了CTRV（1-5行），CA（1-4行），CV（1-3行）模型，每个模型中没有作为状态量变量的都为0，假设为均值为0的随机值wk\text{w}_kwk驱动的。
实际上CA模型和CV模型由于前2项在转弯坐标系中式(1)是非线性形式，而在笛卡尔坐标系内式(2)运动模型是线性的，所以类似方程(1)、(3)的形式一般默认为CT模型，而不用来表示CA、CV运动。

作为式(3)的一种补充，考虑到切向加速度与速度单位矢量τ\boldsymbol \tauτ方向一致，角速度转化成法向加速度w=an/vw={a_n}/{v}w=an/v，则二维平面的CTRA模型也写作：
x˙(t)=vx(t)y˙(t)=vy(t)v˙x(t)=at⋅vxv−anv⋅vyv˙y(t)=at⋅vyv+anv⋅vxa˙t(t)=wt(t)a˙n(t)=wn(t)(2.5)\begin{aligned} &\dot{x}(t)=v_x(t) \\ &\dot{y}(t)=v_y(t)\\ &\dot{v}_x(t)=a_t\cdot \frac{v_x}{v} - \frac{a_n}{v} \cdot v_y \\ &\dot{v}_y(t)=a_t\cdot \frac{v_y}{v} + \frac{a_n}{v} \cdot v_x \\ &\dot{a}_t(t) = \text{w}_t(t) \\ &\dot{a}_n(t) = \text{w}_n(t) \end{aligned}\tag{2.5} x˙(t)=vx(t)y˙(t)=vy(t)v˙x(t)=at⋅vvx−van⋅vyv˙y(t)=at⋅vvy+van⋅vxa˙t(t)=wt(t)a˙n(t)=wn(t)(2.5)

三维空间运动模型

CV

假设惯性系速度不变，考虑速度为随机噪声扰动的CV模型的状态定义为x=[x,y,z,vx,vy,vz]T\mathbf x=[x,y,z,v_x,v_y,v_z]^{\mathrm T}x=[x,y,z,vx,vy,vz]T
x˙(t)=[03I30303]x(t)+[03I3]w(t)\dot{\mathbf{x}}(t)=\left[\begin{array}{ll} \mathbf{0}_{3} & \mathbf{I}_{3} \\ \mathbf{0}_{3} & \mathbf{0}_{3} \end{array}\right] \mathbf{x}(t)+\left[\begin{array}{l} \mathbf{0}_{3} \\ \mathbf{I}_{3} \end{array}\right] \mathbf{w}(t) x˙(t)=[0303I303]x(t)+[03I3]w(t)

CA

假设惯性系加速度为不变，考虑加速度为一阶Markov噪声的CA模型的状态定义为x=[x,y,z,vx,vy,vz,ax,ay,az]T\mathbf x=[x,y,z,v_x,v_y,v_z,a_x,a_y,a_z]^{\mathrm T}x=[x,y,z,vx,vy,vz,ax,ay,az]T
x˙(t)=[03I3030303I30303−I3τa]x(t)+[0303I3]w(t)\dot{\mathbf{x}}(t)=\left[\begin{array}{lll} \mathbf{0}_{3} & \mathbf{I}_{3} & \mathbf{0}_{3} \\ \boldsymbol{0}_{3} & \mathbf{0}_{3}& \mathbf{I}_{3} \\ \boldsymbol{0}_{3} & \mathbf{0}_{3} & - \frac{ \mathbf{I}_{3}}{\tau _a} \end{array}\right] \mathbf{x}(t)+\left[\begin{array}{l} \mathbf{0}_{3} \\ \mathbf{0}_{3} \\ \mathbf{I}_{3} \end{array}\right] \mathbf w(t) x˙(t)=⎣⎡030303I3030303I3−τaI3⎦⎤x(t)+⎣⎡0303I3⎦⎤w(t)若噪声为高斯白噪声，离散化后得到
x(k+1)=[I3Δt⋅I3Δt22I303I3Δt⋅I30303I3][r(k)v(k)a(k)]+[Δt22I3Δt⋅I3I3]w(t){\mathbf{x}}(k+1)=\left[\begin{array}{lll} \mathbf{I}_{3} & \Delta t\cdot\mathbf{I}_{3}& \frac{\Delta t^2}{2}\mathbf{I}_{3} \\ \boldsymbol{0}_{3} & \mathbf{I}_{3}& \Delta t\cdot\mathbf{I}_{3} \\ \boldsymbol{0}_{3} & \boldsymbol{0}_{3} & \mathbf{I}_{3} \end{array}\right] \left[\begin{array}{l} \mathbf{r}(k)\\ \mathbf{v}(k)\\ \mathbf{a}(k) \end{array}\right] + \left[\begin{array}{l} \frac{\Delta t^2}{2} \mathbf{I}_{3} \\ \Delta t\cdot\mathbf{I}_{3} \\ \mathbf{I}_{3} \end{array}\right] \mathbf w(t) x(k+1)=⎣⎡I30303Δt⋅I3I3032Δt2I3Δt⋅I3I3⎦⎤⎣⎡r(k)v(k)a(k)⎦⎤+⎣⎡2Δt2I3Δt⋅I3I3⎦⎤w(t)
考虑噪声为一阶Markov过程，离散化后得到
x(k+1)=[I3Δt⋅I3Δt22I30303I3Δt⋅I3030303I3I3030303(1−1τa)I3][r(k)v(k)a(k)ξ(k)]+[Δt22I3Δt⋅I303I3]w(t){\mathbf{x}}(k+1)=\left[\begin{array}{lll} \mathbf{I}_{3} & \Delta t\cdot\mathbf{I}_{3}& \frac{\Delta t^2}{2}\mathbf{I}_{3} & \mathbf{0}_{3} \\ \boldsymbol{0}_{3} & \mathbf{I}_{3}& \Delta t\cdot\mathbf{I}_{3} & \mathbf{0}_{3}\\ \boldsymbol{0}_{3} & \boldsymbol{0}_{3} & \mathbf{I}_{3}& \mathbf{I}_{3}\\ \boldsymbol{0}_{3} & \boldsymbol{0}_{3} &\boldsymbol{0}_{3} & (1- \frac{1}{\tau _a})\mathbf{I}_{3} \end{array}\right] \left[\begin{array}{l} \mathbf{r}(k)\\ \mathbf{v}(k)\\ \mathbf{a}(k)\\ \mathbf{\xi}(k) \end{array}\right] + \left[\begin{array}{l} \frac{\Delta t^2}{2} \mathbf{I}_{3} \\ \Delta t\cdot\mathbf{I}_{3} \\ \mathbf{0}_{3}\\ \mathbf{I}_{3} \end{array}\right] \mathbf w(t) x(k+1)=⎣⎡I3030303Δt⋅I3I303032Δt2I3Δt⋅I3I3030303I3(1−τa1)I3⎦⎤⎣⎡r(k)v(k)a(k)ξ(k)⎦⎤+⎣⎡2Δt2I3Δt⋅I303I3⎦⎤w(t)
CA模型没有考虑加速度与轨迹的关系。

前2个的状态转移矩阵内没有状态，是线性系统。但CT模型都是非线性系统。

三维恒定速率、角速率（CS-CT）运动

(Constant Speed - Constant Turn Rate)此模型假设目标沿空间弧线运动的速率不变即at=0a_t=0at=0、角速度w\boldsymbol ww不变，则转弯带来加速度a=ω×v\boldsymbol a=\boldsymbol{\omega} \times \boldsymbol va=ω×v。状态定义为x=[x,y,z,vx,vy,vz,wx,wy,wz]T\mathbf x=[x,y,z,v_x,v_y,v_z,w_x,w_y,w_z]^{\mathrm T}x=[x,y,z,vx,vy,vz,wx,wy,wz]T
x˙(t)=[03I30303Ω03030303]x(t)+[0303I3]w(t)(4)\dot{\mathbf{x}}(t)=\left[\begin{array}{lll} \mathbf{0}_{3} & \mathbf{I}_{3} & \mathbf{0}_{3} \\ \boldsymbol{0}_{3} & \mathbf{\Omega} & \mathbf{0}_{3} \\ \boldsymbol{0}_{3} & \mathbf{0}_{3} & \mathbf{0}_{3} \end{array}\right] \mathbf{x}(t)+\left[\begin{array}{l} \mathbf{0}_{3} \\ \mathbf{0}_{3} \\ \mathbf{I}_{3} \end{array}\right] \mathbf{w}(t)\tag{4} x˙(t)=⎣⎡030303I3Ω03030303⎦⎤x(t)+⎣⎡0303I3⎦⎤w(t)(4)其中的矩阵
Ω(w)=[0−ωzωyωz0−ωx−ωyωx0]\mathbf\Omega(\boldsymbol w)=\left[\begin{array}{ccc} 0 & -\omega_{z} & \omega_{y} \\ \omega_{z} & 0 & -\omega_{x} \\ -\omega_{y} & \omega_{x} & 0 \end{array}\right]Ω(w)=⎣⎡0ωz−ωy−ωz0ωxωy−ωx0⎦⎤CT模型由于存在角速度对加速度的牵连加速度项，因而是非线性模型，线性化为离散系统为（假设时间步长为TTT）：
x(k+1)=Φ(k+1,k)x(k)+Γ(k+1,k)w(k)\mathbf{x}(k+1)=\boldsymbol{\Phi}(k+1,k)\mathbf{x}(k)+\boldsymbol{\Gamma}(k+1,k)\mathbf{w}(k) x(k+1)=Φ(k+1,k)x(k)+Γ(k+1,k)w(k)其中的
Φ=[I3B0303I3+A030303I3],Γ=[0303I3],Q=[0303030303030303I3]σ22A=[c1d1−c2ωz−c1ωxωyc2ωy−c1ωxωzc2ωz−c1ωxωyc1d2−c2ωx−c1ωyωz−c2ωy−c1ωxωzc2ωx−c1ωyωzc1d3]B=[c3d1c1ωz−c3ωxωy−c1ωy−c3ωxωz−c1ωz−c3ωxωyc3d2c1ωx−c3ωyωzc1ωy−c3ωxωz−c1ωx−c3ωyωzc3d3]\begin{aligned} \boldsymbol{\Phi} &=\left[\begin{array}{ccc} \mathbf{I}_{3} & \mathbf{B} & \mathbf{0}_{3} \\ 0_{3} & \mathbf{I}_{3}+\mathbf{A} & \mathbf{0}_{3} \\ 0_{3} & \mathbf{0}_{3} & \mathbf{I}_{3} \end{array}\right], \boldsymbol{\Gamma}=\left[\begin{array}{l} \mathbf{0}_{3} \\ \mathbf{0}_{3} \\ \mathbf{I}_{3} \end{array}\right], \mathbf{Q}=\left[\begin{array}{ccc} \mathbf{0}_{3} & \mathbf{0}_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & \mathbf{I}_{3} \end{array}\right] \sigma_{2}^{2} \\ \mathbf{A} &=\left[\begin{array}{ccc} c_{1} d_{1} & -c_{2} \omega_{z}-c_{1} \omega_{x} \omega_{y} & c_{2} \omega_{y}-c_{1} \omega_{x} \omega_{z} \\ c_{2} \omega_{z}-c_{1} \omega_{x} \omega_{y} & c_{1} d_{2} & -c_{2} \omega_{x}-c_{1} \omega_{y} \omega_{z} \\ -c_{2} \omega_{y}-c_{1} \omega_{x} \omega_{z} & c_{2} \omega_{x}-c_{1} \omega_{y} \omega_{z} & c_{1} d_{3} \end{array}\right] \\ \mathbf{B} &=\left[\begin{array}{ccc} c_{3} d_{1} & c_{1} \omega_{z}-c_{3} \omega_{x} \omega_{y} & -c_{1} \omega_{y}-c_{3} \omega_{x} \omega_{z} \\ -c_{1} \omega_{z}-c_{3} \omega_{x} \omega_{y} & c_{3} d_{2} & c_{1} \omega_{x}-c_{3} \omega_{y} \omega_{z} \\ c_{1} \omega_{y}-c_{3} \omega_{x} \omega_{z} & -c_{1} \omega_{x}-c_{3} \omega_{y} \omega_{z} & c_{3} d_{3} \end{array}\right] \end{aligned} ΦAB=⎣⎡I30303BI3+A030303I3⎦⎤,Γ=⎣⎡0303I3⎦⎤,Q=⎣⎡0303030303030303I3⎦⎤σ22=⎣⎡c1d1c2ωz−c1ωxωy−c2ωy−c1ωxωz−c2ωz−c1ωxωyc1d2c2ωx−c1ωyωzc2ωy−c1ωxωz−c2ωx−c1ωyωzc1d3⎦⎤=⎣⎡c3d1−c1ωz−c3ωxωyc1ωy−c3ωxωzc1ωz−c3ωxωyc3d2−c1ωx−c3ωyωz−c1ωy−c3ωxωzc1ωx−c3ωyωzc3d3⎦⎤文献【3】推导出了参数d1=ωy2+ωz2,d2=ωx2+ωz2,d3=ωx2+ωy2,c1=cos⁡ΩT−1Ω2,c2=sin⁡ΩTΩ,c3=1Ω2(sin⁡ΩTΩ−T).\begin{aligned} &d_{1}=\omega_{y}^{2}+\omega_{z}^{2} \quad, \quad d_{2}=\omega_{x}^{2}+\omega_{z}^{2} \quad, \quad d_{3}=\omega_{x}^{2}+\omega_{y}^{2}, \\ &c_{1}=\frac{\cos \Omega T-1}{\Omega^{2}}, c_{2}=\frac{\sin \Omega T}{\Omega}, c_{3}=\frac{1}{\Omega^{2}}\left(\frac{\sin \Omega T}{\Omega}-T\right) . \end{aligned}d1=ωy2+ωz2,d2=ωx2+ωz2,d3=ωx2+ωy2,c1=Ω2cosΩT−1,c2=ΩsinΩT,c3=Ω21(ΩsinΩT−T).以及Ω=∥w∥\Omega=\| \boldsymbol w\|Ω=∥w∥. 需要注意，这个模型的非线性程度非常高，在使用的时候三轴角速度的估计比较困难。

三维平面运动CTRV模型

在CS-CT模型中尚未考虑平面运动假设，它可以三维螺线运动。

下面的这种是更普遍认同的（Constant Turn Rate and Speed，CTRV）模型，对应于二维CTRV运动。包含了

三维恒定速率、
恒定角速率、
平面运动

假设，平面运动假设即角速度变化率沿着角速度方向
ω˙=k⋅ω∥ω∥=−1τwω\dot{\boldsymbol\omega} =k\cdot \frac{\boldsymbol\omega}{\|\boldsymbol\omega\|} =-\frac{1}{\tau _w}\boldsymbol\omegaω˙=k⋅∥ω∥ω=−τw1ω

其中k(t),τw(t)k(t),\tau_w(t)k(t),τw(t)均为待估计的状态量。如果恒定速率、平面运动同时考虑，则为以下模型
x˙(t)=vx(t)y˙(t)=vy(t)z˙(t)=vz(t)v˙x(t)=wy⋅vz−wz⋅vyv˙y(t)=wz⋅vx−wx⋅vzv˙z(t)=wx⋅vy−wy⋅vxω˙(t)=−1τwω(t)τw˙=wτ(5)\begin{aligned} &\dot{x}(t)=v_x(t) \\ &\dot{y}(t)=v_y(t)\\ &\dot{z}(t)=v_z(t)\\ &\dot{v}_x(t)=w_y\cdot v_z-w_z \cdot v_y\\ &\dot{v}_y(t)=w_z\cdot v_x-w_x \cdot v_z\\ &\dot{v}_z(t)=w_x\cdot v_y-w_y \cdot v_x\\ &\dot{\boldsymbol\omega}(t)=-\frac{1}{\tau _w}\boldsymbol\omega(t) \\ &\dot{\tau _w}=w_{\tau} \end{aligned}\tag{5} x˙(t)=vx(t)y˙(t)=vy(t)z˙(t)=vz(t)v˙x(t)=wy⋅vz−wz⋅vyv˙y(t)=wz⋅vx−wx⋅vzv˙z(t)=wx⋅vy−wy⋅vxω˙(t)=−τw1ω(t)τw˙=wτ(5)其中角速度w\boldsymbol ww方向不变、大小变化，加速度只包含旋转加速度a=ω×v\boldsymbol a = \boldsymbol{\omega} \times \boldsymbol va=ω×v。此模型需要估计目标的位置、惯性系速度、角速度、角速度变化率，即x=[x,y,z,vx,vy,vz,wx,wy,wz,τw]T\boldsymbol x=[x,y,z,v_x,v_y,v_z,w_x,w_y,w_z,\tau _w]^{\mathrm T}x=[x,y,z,vx,vy,vz,wx,wy,wz,τw]T

三维弹道系分解的CTRA模型

在三维空间平面内的CTRA模型为，
x˙(t)=vx(t)y˙(t)=vy(t)z˙(t)=vz(t)v˙x(t)=at⋅vxv+wy⋅vz−wz⋅vyv˙y(t)=at⋅vyv+wz⋅vx−wx⋅vzv˙z(t)=at⋅vzv+wx⋅vy−wy⋅vxω˙(t)=wwa˙t(t)=wa(5)\begin{aligned} &\dot{x}(t)=v_x(t) \\ &\dot{y}(t)=v_y(t)\\ &\dot{z}(t)=v_z(t)\\ &\dot{v}_x(t)=a_t\cdot \frac{v_x}{v}+w_y\cdot v_z-w_z \cdot v_y\\ &\dot{v}_y(t)=a_t\cdot \frac{v_y}{v}+w_z\cdot v_x-w_x \cdot v_z\\ &\dot{v}_z(t)=a_t\cdot \frac{v_z}{v}+w_x\cdot v_y-w_y \cdot v_x\\ &\dot{\boldsymbol\omega}(t)=\mathbf{w}_w\\ &\dot{a}_t(t)=\text{w}_a \end{aligned}\tag{5} x˙(t)=vx(t)y˙(t)=vy(t)z˙(t)=vz(t)v˙x(t)=at⋅vvx+wy⋅vz−wz⋅vyv˙y(t)=at⋅vvy+wz⋅vx−wx⋅vzv˙z(t)=at⋅vvz+wx⋅vy−wy⋅vxω˙(t)=wwa˙t(t)=wa(5)其中at≠0a_t\neq0at=0、角速度w\boldsymbol ww不变，则加速度由2部分构成a=at⋅τ+ω×v\boldsymbol a = a_t\cdot \boldsymbol \tau+ \boldsymbol{\omega} \times \boldsymbol va=at⋅τ+ω×v。CTRA模型需要估计目标的位置、惯性系速度、角速度、切向加速度，即x=[x,y,z,vx,vy,vz,wx,wy,wz,at]T\boldsymbol x=[x,y,z,v_x,v_y,v_z,w_x,w_y,w_z,a_t]^{\mathrm T}x=[x,y,z,vx,vy,vz,wx,wy,wz,at]T。但公式(5)比较少见，更常用的是三维空间的CA模型。更进一步的力学内容，可参考论文【2】.

平面ACT模型

假设目标的运动轨迹基本上在一个平面内，那么它的角速度朝向是始终不变的，这样是三维CT模型的一个更常用的形式。如果恒定速率、平面运动同时考虑，则为以下模型
x˙(t)=vx(t)y˙(t)=vy(t)v˙x(t)=wz⋅vyv˙y(t)=−wz⋅vxwz˙=ξw(t)(5)\begin{aligned} &\dot{x}(t)=v_x(t) \\ &\dot{y}(t)=v_y(t)\\ &\dot{v}_x(t)=w_z\cdot v_y\\ &\dot{v}_y(t)=-w_z \cdot v_x\\ &\dot{w_z}=\xi_w(t) \end{aligned}\tag{5} x˙(t)=vx(t)y˙(t)=vy(t)v˙x(t)=wz⋅vyv˙y(t)=−wz⋅vxwz˙=ξw(t)(5)其中角速度ω\omegaω受有色噪声或白噪声驱动，加速度只包含旋转加速度a=ω×v\boldsymbol a = \boldsymbol{\omega} \times \boldsymbol va=ω×v。模型中包含转弯角速度，这种模型被称为扩维的转弯模型(Augmented Constant Turn, ACT)。此模型需要估计目标的平面位置、速度、转动角速度，即x=[x,y,vx,vy,wz]T\boldsymbol x=[x,y,v_x,v_y,w_z]^{\mathrm T}x=[x,y,vx,vy,wz]T，此时方程的离散形式为：
[x(k+1)y(k+1)vx(k+1)vy(k+1)ω(k+1)]=[x+1ωsin⁡(ωT)vx−1ω(1−cos⁡(ωT))vyy+1ω(1−cos⁡(ωT))vx+1ω(sin⁡(ωT))vyvxcos⁡(ωT)−vysin⁡(ωT)vxsin⁡(ωT)+vycos⁡(ωT)0]+Γw\left[\begin{array}{ccccc} x(k+1)\\ y(k+1)\\ v_x(k+1)\\ v_y(k+1)\\ \omega(k+1) \end{array}\right]= \left[\begin{array}{c} x+\frac{1}{\omega} \sin (\omega T)v_x-\frac{1}{\omega}(1-\cos (\omega T))v_y \\ y+\frac{1}{\omega}(1-\cos (\omega T))v_x+\frac{1}{\omega}(\sin (\omega T))v_y \\ v_x \cos (\omega T)-v_y \sin (\omega T) \\ v_x \sin (\omega T)+v_y \cos (\omega T) \\ 0 \end{array}\right] + \Gamma \mathbf w ⎣⎡x(k+1)y(k+1)vx(k+1)vy(k+1)ω(k+1)⎦⎤=⎣⎡x+ω1sin(ωT)vx−ω1(1−cos(ωT))vyy+ω1(1−cos(ωT))vx+ω1(sin(ωT))vyvxcos(ωT)−vysin(ωT)vxsin(ωT)+vycos(ωT)0⎦⎤+Γw考虑角速度为常值且由白噪声驱动。在这之中，ACT模型的转动角速度直接决定跟踪精度，所以如果为了收敛更快，会把加速度、角速度中均包含噪声项w=[wax,way,ww]T\mathbf w=[w_{ax},w_{ay},w_w]^{\mathrm T}w=[wax,way,ww]T：
Γw=[Δt22⋅I202×1Δt⋅I202×101×21][waxwayww]\Gamma \mathbf w= \left[\begin{array}{cc} \frac{\Delta t^{2}}{2} \cdot \boldsymbol{I}_{2} & 0_{2 \times 1} \\ \Delta t \cdot \boldsymbol{I}_{2} & 0_{2 \times 1} \\ 0_{1 \times 2} & 1 \end{array}\right] \left[\begin{array}{ccccc} w_{ax}\\ w_{ay}\\ w_w \end{array}\right] Γw=⎣⎡2Δt2⋅I2Δt⋅I201×202×102×11⎦⎤⎣⎡waxwayww⎦⎤

如果角速度由一阶Markov过程噪声驱动，那么它的表达式为：
ω˙=−1τwω+ww\dot\omega =-\frac{1}{\tau _w}\omega + w_wω˙=−τw1ω+ww其中τw\tau _wτw为相关噪声的时间相关系数，与时间同量纲。这个式子的离散形式为：
ω(k+1)=exp⁡(Tτw)ω(k)+ww(k)\omega(k+1) = \exp(\frac{T}{\tau_w})\omega(k) + w_w(k)ω(k+1)=exp(τwT)ω(k)+ww(k)由于系数τw{\tau_w}τw是人为设定的，因此这个式子也写作
ω(k+1)=α⋅ω(k)+ww(k)\omega(k+1) =\alpha\cdot\omega(k) + w_w(k)ω(k+1)=α⋅ω(k)+ww(k)其中α∈(0,1)\alpha\in(0,1)α∈(0,1)

参考

CSDN博客3月16日 CV,CA,CTRV等运动模型，EKF，UKF在运动模型下的分析与实践
Li, X. R., & Jilkov, V. P. (2003). Survey of maneuvering target tracking. Part I. Dynamic models. IEEE Transactions on Aerospace and Electronic Systems, 39(4), 1333-1364. doi:10.1109/TAES.2003.1261132
Ronghui, Z., & Wan, J. (2006). Passive maneuvering target tracking using 3D constant-turn model. 2006 IEEE Conference on Radar. doi:10.1109/RADAR.2006.1631832