Singer模型与CT模型状态转移矩阵的求解

Singer模型与CT模型状态转移矩阵的求解

前言

回想起来，第一次接触Singer模型与CT模型时的状态转移矩阵时，对求解过程一知半解。现在，将推导过程记录下来，温故知新。若有纰漏，欢迎讨论。

状态方程

当我们对一个线性时不变系统的输入输出与状态变量感兴趣时，状态空间方程是一种常用的手段。状态空间方程由状态方程与输出方程构成，可以表述为
$$\{\begin{array}{cc}\dot{\mathbf{x}}=\mathbf{Ax}+\mathbf{Bu}& 状态方程\\ \mathbf{y}=\mathbf{Cx}+\mathbf{Du}& 输出方程\end{array}$$
其中，状态方程是一个一阶微分方程。$\mathbf{x}$与$\dot{\mathbf{x}}$表示状态向量及其一阶导数。$\mathbf{u}$为系统的外部输入。$\mathbf{A}$表示系统矩阵。$\mathbf{B}$表示输入矩阵。$\mathbf{y}$为系统输出。$\mathbf{C}$为输出矩阵。$\mathbf{D}$为直接输出矩阵。

我们只对状态方程感兴趣。现在为了简化问题，我们并不考虑外部输入对系统的影响，因此，状态方程简化为
$$\dot{\mathbf{x}}=\mathbf{Ax}$$

使用拉普拉斯变换对上述一阶微分方程进行求解。这里补充几个常用的拉普拉斯变换对
$$\begin{array}{rl}\mathcal{L}(x(t))& =X(s)\\ \mathcal{L}(\dot{x}(t))& =sX(s)-x(0)\\ \mathcal{L}(e^{at})& =\frac{1}{s-a}\\ \mathcal{L}(1)& =\frac{1}{s}\\ \mathcal{L}(t)& =\frac{1}{s^2}\\ \mathcal{L}(cos\omega t)& =\frac{s}{s^2+{\omega }^2}\\ \mathcal{L}(sin\omega t)& =\frac{\omega }{s^2+{\omega }^2}\\ \mathcal{L}(e^{-\alpha t}sin(\omega t))& =\frac{\omega }{(s+\alpha {)}^2+{\omega }^2}\end{array}$$

现在对简化的状态方程的等式两端进行拉普拉斯变换并进行变换，可得状态方程的解
$$\begin{array}{rl}\mathcal{L}\left(\dot{\mathbf{x}}(t)\right)& =\mathcal{L}\left(\mathbf{Ax}(t)\right)\\ s\mathbf{X}(s)-\mathbf{x}(0)& =\mathbf{AX}(s)\\ (s-\mathbf{A})\mathbf{X}(s)& =\mathbf{x}(0)\\ \mathbf{X}(s)& =\frac{\mathbf{x}(0)}{s-\mathbf{A}}\\ {\mathcal{L}}^{-1}\left(\mathbf{X}\left(s\right)\right)& ={\mathcal{L}}^{-1}\left(\frac{\mathbf{x}(0)}{s-\mathbf{A}}\right)\\ \mathbf{x}(t)& =e^{\mathbf{A}t}\mathbf{x}(0)\end{array}$$

现在我们得到了线性时不变系统的状态方程的解
$$\mathbf{x}(t)=e^{\mathbf{A}t}\mathbf{x}(0)$$
该解表述了$t$时刻的状态向量$\mathbf{x}(t)$与初始状态向量$\mathbf{x}(0)$的关系。$e^{\mathbf{A}t}$为矩阵指数函数。该解是在连续时间系统下的解。通常，我们更关心在离散时间系统下的解。假设离散时间系统的时间维度采样率为$\Delta T$，则$T$时刻与$T+\Delta T$时刻的状态向量分别为

$$\begin{array}{rl}\mathbf{x}(T)& =e^{\mathbf{A}T}\mathbf{x}(0)\\ \mathbf{x}(T+\Delta T)& =e^{\mathbf{A}(T+\Delta T)}\mathbf{x}(0)\end{array}$$

对上述两式求比值可得$x(T)$与$x(T+\Delta T)$的关系
$$\mathbf{x}(T+\Delta T)=e^{\mathbf{A}\Delta T}\mathbf{x}(T)$$
这就是我们熟知的状态转移方程，其中$e^{\mathbf{A}\Delta T}$为状态转移矩阵。

矩阵指数函数

现在我们已经推导出了离散时间系统下的状态转移方程，但是矩阵指数函数的存在妨碍我们在实际应用中的使用。矩阵指数函数的解法有许多种，这里简单介绍其中的两种。

泰勒展开

实数的泰勒展开我们已经十分熟悉，
$$e^{at}=1+(at)+\frac{1}{2!}(at{)}^2+\frac{1}{3!}(at{)}^3+\frac{1}{4!}(at{)}^4+......$$
矩阵指数函数的泰勒展开的形式类似
$$e^{\mathbf{A}t}=1+(\mathbf{A}t)+\frac{1}{2!}(\mathbf{A}t{)}^2+\frac{1}{3!}(\mathbf{A}t{)}^3+\frac{1}{4!}(\mathbf{A}t{)}^4+......$$

注意，如果用有限项求和的方式代替矩阵指数函数的解，则只能得到近似解。

拉普拉斯变换

使用拉普拉斯变换的形式，同样可以得到矩阵指数函数的解。

$$e^{\mathbf{A}t}={\mathcal{L}}^{-1}(\frac{1}{s\mathbf{I}-\mathbf{A}})$$

Singer模型

常规的运动模型包括CV模型与CA模型。但是，当目标发生机动运动时(简单理解为目标加速度的大小与方向发生了变化),CV模型与CA模型便不再适用。Singer引入了机动频率$\alpha$来修正传统的模型。当机动频率$\alpha$越高，意味着目标加速度变化越快。反之亦然。当机动频率为零时，目标不再处于机动状态。此时，可以根据当前目标加速度的大小，使用CV模型或CA模型对其进行描述。Singer认为目标的加速度的状态方程可以描述为
$$\dot{a}(t)=-\alpha a(t)+\tilde{v}(t)$$
其中，$a(t)$与$\dot{a}(t)$为t时刻的加速度与加加速度。$\tilde{v}$为一零均值的高斯白噪声。可以发现，状态方程的描述与我们对机动频率的阐述在数值变化趋势上是一致的。当机动频率固定时，目标加加速度的大小取决于当前加速度的大小。由于该方程是一个一阶微分方程，我们同样使用拉普拉斯变换的方法对方程求解。这里暂且忽略噪声的影响，由此得到解的形式为
$$a(t)=e^{-\alpha t}a(0)$$
其中，$a(0)$为初始加速度大小。根据上式，加速度的自相关函数为
$$R(\tau )=E[a(t)a(t+\tau )]=\sigma e^{-\alpha |\tau |}$$
其中，$\sigma$为加速度方差。由此可见，Singer模型下的加速度自相关函数是一个指数函数。

Singer模型的状态方程可以表述为
$$\dot{\mathbf{X}}=\mathbf{AX}+\tilde{\mathbf{V}}$$
其中，状态向量$\mathbf{X}=[x,\dot{x},\ddot{x}{]}^{{}^{\prime }}$，$x$表示位置信息。系统矩阵为
$$\mathbf{A}=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& -\alpha \end{array}\right]$$
其中，$\alpha$为机动频率。过程噪声矩阵为
$$\tilde{\mathbf{V}}=\left[\begin{array}{c}0\\ 0\\ \tilde{v}\end{array}\right]$$
因此，状态方程可以重写为
$$\left[\begin{array}{c}\dot{x}\\ \ddot{x}\\ \dot{a}\end{array}\right]=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& -\alpha \end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ \ddot{x}\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ \tilde{v}\end{array}\right]$$
其中，$\ddot{x}=a$。使用拉普拉斯变换求解状态方程的解，可得
$$\begin{array}{rl}{e}^{\mathbf{A}t}& ={\mathcal{L}}^{-1}(\frac{1}{s\mathbf{I}-\mathbf{A}})\\ & ={\mathcal{L}}^{-1}({\left[\begin{array}{ccc}s& -1& 0\\ 0& s& -1\\ 0& 0& s+\alpha \end{array}\right]}^{-1})\\ & ={\mathcal{L}}^{-1}(\left[\begin{array}{ccc}\frac{1}{s}& \frac{1}{s^2}& \frac{1}{s^2(s+\alpha )}\\ 0& \frac{1}{s}& \frac{1}{s(s+\alpha )}\\ 0& 0& \frac{1}{s+\alpha }\end{array}\right])\\ & =\left[\begin{array}{ccc}1& t& \frac{\alpha t-1+e^{-\alpha t}}{{\alpha }^2}\\ 0& 1& \frac{1-e^{-\alpha t}}{\alpha }\\ 0& 0& e^{-\alpha t}\end{array}\right]\end{array}$$

在离散时间系统下，假设采样间隔为$T$,则状态转移矩阵为
$$\mathbf{F}=\left[\begin{array}{ccc}1& T& \frac{\alpha T-1+e^{-\alpha T}}{{\alpha }^2}\\ 0& 1& \frac{1-e^{-\alpha T}}{\alpha }\\ 0& 0& e^{-\alpha T}\end{array}\right]$$

CT模型

二维目标运动学方程为
$$\{\begin{array}{c}\dot{x}(t)=v(t)cos\varphi (t)\\ \dot{y}(t)=v(t)sin\varphi (t)\\ \dot{v}(t)=a_t(t)\\ \dot{\varphi }(t)=\frac{a_n(t)}{v(t)}\end{array}$$
其中，$x,y$为笛卡尔坐标系下的坐标，$v$为空间速度，$a_t(t)$为切向加速度(沿着速度方向)，$a_n(t)$为法相加速度，$\varphi$为航向角。若令$a_n(t)\ne 0,a_t(t)=0$，则此时目标进行匀速曲线运动。若此时$a_n(t)$为一常数，则此时目标进行匀速转弯运动。令状态向量$\mathbf{X}=[x,\dot{x},y,\dot{y}{]}^{{}^{\prime }}$，且角速度$\omega =\dot{\varphi }$。$x$维度的加速度可以表示为
$$\begin{array}{rl}\ddot{x}(t)& =a_t(t)cos\varphi (t)-\omega v(t)sin\varphi (t)\\ & =-\omega \dot{y}(t)\end{array}$$
同理$$\ddot{y}(t)=\omega \dot{x}(t)$$
因此系统矩阵可以写为
$$\mathbf{A}=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& 0& -\omega \\ 0& 0& 0& 1\\ 0& \omega & 0& 0\end{array}\right]$$
使用拉普拉斯变换求解状态方程的解，可得
$$\begin{array}{rl}{e}^{\mathbf{A}t}& ={\mathcal{L}}^{-1}(\frac{1}{s\mathbf{I}-\mathbf{A}})\\ & ={\mathcal{L}}^{-1}({\left[\begin{array}{cccc}s& -1& 0& 0\\ 0& s& 0& \omega \\ 0& 0& s& -1\\ 0& -\omega & 0& s\end{array}\right]}^{-1})\\ & ={\mathcal{L}}^{-1}(\left[\begin{array}{cccc}\frac{1}{s}& \frac{1}{s^2+{\omega }^2}& 0& \frac{-\omega }{s^3+s{\omega }^2}\\ 0& \frac{s}{s^2+{\omega }^2}& 0& \frac{-\omega }{s^2+{\omega }^2}\\ 0& \frac{\omega }{s^3+s{\omega }^2}& \frac{1}{s}& \frac{1}{s^2+{\omega }^2}\\ 0& \frac{\omega }{s^2+{\omega }^2}& 0& \frac{s}{s^2+{\omega }^2}\end{array}\right])\\ & =\left[\begin{array}{cccc}1& \frac{sin\omega t}{\omega }& 0& -\frac{1-cos\omega t}{\omega }\\ 0& cos\omega t& 0& -sin\omega t\\ 0& \frac{1-cos\omega t}{\omega }& 1& \frac{sin\omega t}{\omega }\\ 0& sin\omega t& 0& cos\omega t\end{array}\right]\end{array}$$

在离散时间系统下，假设采样间隔为$T$,则状态转移矩阵为
$$\mathbf{F}=\left[\begin{array}{cccc}1& \frac{sin\omega T}{\omega }& 0& -\frac{1-cos\omega T}{\omega }\\ 0& cos\omega T& 0& -sin\omega T\\ 0& \frac{1-cos\omega T}{\omega }& 1& \frac{sin\omega T}{\omega }\\ 0& sin\omega T& 0& cos\omega T\end{array}\right]$$