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模型

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& \frac{1}{s^2}& \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& \frac{1}{s^2}& \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& 1\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 }^2}\\ 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 }^2}\\ 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]$$