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?(t)\\ \dot{y}(t)=v(t)sin?(t)\\ \dot{v}(t)=a_t(t)\\ \dot{?}(t)=\frac{a_n(t)}{v(t)}\end{array}$$
其中，$x,y$為笛卡爾坐標系下的坐標，$v$為空間速度，$a_t(t)$為切向加速度(沿著速度方向)，$a_n(t)$為法相加速度，$?$為航向角。若令$a_n(t)\ne 0,a_t(t)=0$，則此時目標進行勻速曲線運動。若此時$a_n(t)$為一常數，則此時目標進行勻速轉彎運動。令狀態向量$\mathbf{X}=[x,\dot{x},y,\dot{y}{]}^{{}^{\prime }}$，且角速度$\omega =\dot{?}$。$x$維度的加速度可以表示為
$$\begin{array}{rl}\ddot{x}(t)& =a_t(t)cos?(t)?\omega v(t)sin?(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]$$