线性连续时间状态空间模型的离散化（Discretization of Linear Continuous-Time State-Space Models）

1 .状态空间模型

非线性连续时间状态空间模型
$$\begin{array}{rl}\dot{\mathbf{x}}(t)& =f(x(t))+{\mathbf{B}}_w(x(t))\mathbf{w}(t)\\ {\mathbf{y}}_n& =g(x_n)+r_n\end{array}$$
线性离散时间状态空间模型
$$\begin{array}{rl}{\mathbf{x}}_n& =\mathbf{F}{x}_{n-1}+{\mathbf{B}}_q{\mathbf{q}}_n\\ {\mathbf{y}}_n& =\mathbf{G}{x}_n+{\mathbf{r}}_n\end{array}$$
非线性离散时间状态空间模型
$$\begin{array}{rl}{\mathbf{x}}_n& =f(x_{n-1})+{\mathbf{B}}_q(x_{n-1}){\mathbf{q}}_n\\ {\mathbf{y}}_n& =g(x_n)+{\mathbf{r}}_n\end{array}$$

示例：Quasi-Constant Turn Model in 2D 二维准常数转弯模型

• 车辆在位置$p(t)$, 速度为$v(t)$, 方位角（航向角）$\phi (t)$
• 状态向量 $x=\left[\begin{array}{c}{p}^x(t)\\ p^y(t)\\ v(t)\\ \phi (t)\end{array}\right]$
• 动态模型
  $$\left[\begin{array}{c}{\dot{p}}^x(t)\\ {\dot{p}}^y(t)\\ \dot{v}(t)\\ \dot{\phi }(t)\end{array}\right]=\left[\begin{array}{c}v(t)\cos (\phi (t))\\ v(t)\sin (\phi (t))\\ 0\\ 0\end{array}\right]+\left[\begin{array}{cc}0& 0\\ 0& 0\\ 1& 0\\ 0& 1\end{array}\right]w(t)$$

2. 连续时间的离散化模型

我们为什么使用连续时间的离散化模型？以下有几个原因，

• 在计算机中实现传感器融合
• 数据只能在离散时间步上被处理 $t_1,t_2,...,t_n$
• 离散的连续时间模型和离散时间模型关系密切
• 举例， 车辆跟踪

所以，离散的连续事件模型等于解决在时间$t_{n-1}\to t_n$间的ODE(常微分方程)/SDE(随机微分方程)模型。

常微分方程wiki

在数学中，常微分方程（ODE）是一个包含一个或多个独立变量的函数和这些函数的导数的微分方程。普通方程与偏微分方程形成对比，后者可能涉及一个以上的独立变量。

随机微分方程wiki

随机微分方程（SDE）是一个微分方程，其中一个或多个项是一个随机过程，导致其解决方案也是一个随机过程。SDE被用来模拟各种现象，如不稳定的股票价格或受热波动影响的物理系统。通常情况下，SDE包含一个变量，代表随机白噪声，作为布朗运动或维纳过程的导数计算。然而，其他类型的随机行为也是可能的，如跳跃过程。

3. 线性一阶ODE模型

我们的目标是在区间$(t_{n-1},t_n]$中解一阶线性常微分方程
$$\dot{x}(t)=ax(t)+bu(t)$$
假设， 将上式乘以积分因子$e^{-at}$
$$e^{-at}\dot{x}=e^{-at}ax(t)+e^{-at}bu(t)$$
在$t$可分的ODE，
$$\frac{\mathrm{d}}{\mathrm{d}t}{e}^{-at}x(t)=e^{-at}bu(t)$$
解为，
$$e^{-a{t}_n}x(t_n)-e^{-a{t}_{n-1}}x(t_{n-1})={\int }_{t_{n-1}}^{t_n}{e}^{-at}bu(t)\mathrm{d}t$$
重新整理可得
$$x(t_n)=e^{a\Delta t}x(t_{n-1})+{\int }_{t_{n-1}}^{t_n}{e}^{a(t_n-t)}bu(t)\mathrm{d}t$$

推广到线性一阶ODE矢量模型
$$\dot{x}(t)=\mathbf{A}\mathbf{x}(t)+{\mathbf{B}}_{u}u(t)$$

我们需要计算积分因子，这里引入矩阵指数的定义：

指数的定义
$$e^a=\sum _{k=0}^{\infty }\frac{1}{k}{a}^k$$
矩阵指数的定义
$$e^A=\sum _{k=0}^{\infty }\frac{1}{k!}{A}^k$$
矩阵指数相对于标量x的导数。
$$\frac{\mathrm{d}}{\mathrm{d}x}{e}^{\mathbf{A}\mathbf{x}}=e^{\mathbf{A}\mathbf{x}}A$$
$A^T$的矩阵指数:
$$(e^A{)}^{\mathbf{T}}=e^{A^T}$$

解线性一阶向量ODEs
一般线性动态模型:
$$\dot{x}(t)=\mathbf{A}\mathbf{x}(t)+{\mathbf{B}}_{u}u(t)$$
乘以积分系数$e^{-\mathbf{A}t}$
$$e^{-\mathbf{A}t}\dot{x}(t)=e^{-\mathbf{A}t}\mathbf{A}\mathbf{x}(t)+e^{-\mathbf{A}t}{\mathbf{B}}_{u}u(t)$$
重新整理得，
$$e^{-\mathbf{A}t}\dot{x}(t)-e^{-\mathbf{A}t}\mathbf{A}\mathbf{x}(t)=e^{-\mathbf{A}t}{\mathbf{B}}_{u}u(t)$$
使用$\frac{\mathrm{d}}{\mathrm{d}t}{e}^{-\mathbf{A}t}x(t)=e^{-\mathbf{A}t}\dot{x}(t)+e^{-A}\mathbf{A}\mathbf{x}(t)$替代得(在$t$时)
$$\frac{\mathrm{d}}{\mathrm{d}t}{e}^{-\mathbf{A}t}x(t)=e^{-\mathbf{A}t}{\mathbf{B}}_{u}u(t)$$
根据$t$的积分
$$\begin{array}{rl}{\int }_{t_{n-1}}^{t_n}\mathrm{d}[e^{-\mathbf{A}t}x(t)]& ={\int }_{t_{n-1}}^{t_n}{e}^{-\mathbf{A}t}{\mathbf{B}}_{u}u(t)\mathrm{d}t\\ [e^{-\mathbf{A}t}x(t){]}_{t=t_{n-1}}^{t_n}& ={\int }_{t_{n-1}}^{t_n}{e}^{-\mathbf{A}t}{\mathbf{B}}_{u}u(t)\mathrm{d}t\\ e^{-\mathbf{A}{t}_n}x(t_n)-e^{-\mathbf{A}{t}_{n-1}}x(t_{n-1})& ={\int }_{t_{n-1}}^{t_n}{e}^{-\mathbf{A}t}{\mathbf{B}}_{u}u(t)\mathrm{d}t\end{array}$$
整理得，
$$x(t_n)=e^{\mathbf{A}(t_n-t_{n-1})}x(t_{n-1})+{\int }_{t_{n-1}}^{t_n}{e}^{\mathbf{A}(t_n-t)}{\mathbf{B}}_{u}u(t)\mathrm{d}t$$

输入的$u(t)$在不同的采样实例之间通常是恒定的(零阶保持；zero-order-hold(ZOH)) 。

然后有，
$${\int }_{t_{n-1}}^{t_n}{e}^{\mathbf{A}(t_n-t)}{\mathbf{B}}_{u}u(t)\mathrm{d}t={\int }_{t_{n-1}}^{t_n}{e}^{\mathbf{A}(t_n-t)}{\mathbf{B}}_{u}u(t)\mathrm{d}t{\mathbf{u}}_{n-t}$$

离散确定性线性动态模型
线性连续时间动态模型
$$\dot{x}(t)=\mathbf{A}\mathbf{x}(t)+{\mathbf{B}}_{u}u(t)$$
离散动态模型
$${\mathbf{x}}_n={\mathbf{F}}_n{\mathbf{x}}_{n-1}+{\mathbf{L}}_n{\mathbf{u}}_{n-1}$$
其中
$$\begin{array}{rl}{\mathbf{F}}_n& \triangleq e^{\mathbf{A}(t_n-t_{n-1})}\\ {\mathbf{L}}_n& \triangleq {\int }_{t_{n-1}}^{t_n}{e}^{\mathbf{A}(t_n-t)}{\mathbf{B}}_u\mathrm{d}t\end{array}$$
可以看到，离散的动态模型完全等于连续时间模型。

示例1：一维确定性线性动态模型

动态模型
$$\left[\begin{array}{c}\dot{p}(t)\\ \dot{v}(t)\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\left[\begin{array}{c}p(t)\\ v(t)\end{array}\right]+\left[\begin{array}{c}0\\ 1\end{array}\right]u(t)$$
使用如下方程
$${\mathbf{F}}_n=e^{\mathbf{A}\Delta t}=\sum _{j=0}^{\infty }\frac{1}{j!}{A}^j(\Delta t{)}^j$$
$\mathbf{A}$的值
$$\begin{array}{rl}{\mathbf{A}}^0& =\mathbf{I}\\ {\mathbf{A}}^1& =\mathbf{A}\\ {\mathbf{A}}^j& =0j\ge 2\end{array}$$
然后有
$$\begin{array}{rl}{\mathbf{A}}_{d}d& =\sum _{n=0}^{\infty }\frac{1}{n!}{A}^n(\Delta t{)}^n\\ & =\frac{1}{0!}I(\Delta t{)}^0+\frac{1}{1!}\mathbf{A}\Delta t+\frac{1}{2!}{\mathbf{A}}^2(\Delta t{)}^2+\frac{1}{3!}{\mathbf{A}}^3(\Delta t{)}^3+\frac{1}{4!}{\mathbf{A}}^4(\Delta t{)}^4\\ & =\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\end{array}$$
输入矩阵
$$\mathbf{L}={\int }_{t_{n-1}}^{t_n}{e}^{\mathbf{A}(t_n-t)}{\mathbf{B}}_u\mathrm{d}t$$
其中，
$$e^{\mathbf{A}(t_n-t)}=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right],{\mathbf{B}}_u=\left[\begin{array}{c}0\\ 1\end{array}\right]$$
得到，连续时间模型
$$\left[\begin{array}{c}\dot{p}(t)\\ \dot{v}(t)\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\left[\begin{array}{c}p(t)\\ v(t)\end{array}\right]+\left[\begin{array}{c}0\\ 1\end{array}\right]u(t)$$
离散时间模型
$${\mathbf{x}}_n=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]{\mathbf{x}}_{n-1}+\left[\begin{array}{c}\frac{(\Delta t{)}^2}{2}\\ \Delta t\end{array}\right]{\mathbf{u}}_{n-1}$$

示例2：刚体平面运动学模型

刚体在平面中的运动可以使用以下方程定义：
$$\begin{array}{rl}{a}_x& ={\dot{v}}_x-r{v}_y\\ a_y& ={\dot{v}}_y+r{v}_x\end{array}$$
其中状态空间表示为：
$$\begin{array}{rl}{\dot{x}}_k& =A_k(t)x_k+B_k(t)u_k\\ y_k& =C_k(t)x_k\end{array}$$
其中，
$$\begin{array}{rl}{A}_k(t)& =\left[\begin{array}{cc}0& -r(t)\\ r(t)& 0\end{array}\right],B_k(t)=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],\\ C_k(t)& =\left[\begin{array}{cc}1& 0\end{array}\right]\end{array}$$
离散化后的模型为：
$$\begin{array}{rl}{\hat{\text{x}}}_k[t+1]& =A_{k,d}[t]\cdot {\hat{\text{x}}}_k[t]+B_{k,d}[t]\cdot u_k[t]\\ {\text{y}}_k[t]& =C_{k,d}[t]\cdot {\hat{\text{x}}}_k[t]\end{array}$$
我们可以借助MATLAB来计算离散化后的系统矩阵$A_{k,d}$和$B_{k,d}$：

% Create symbolic variables
syms r 'real';
syms dt 'real';

% define system matrices
A_k = [0 -r ;r 0];
B_k = [1 0; 0 1];
I = [1 0; 0 1];

% calculate discretised system matrices
A_kd = I + A_k*dt + 1/2*A_k^2*dt^2 + 1/6*A_k^3*dt^3 + 1/24*A_k^4*dt^4
B_kd = simplify(int(expm(dt*A_k)*B_k, dt))

可以得到：

A_kd =
[(dt^4*r^4)/24 - (dt^2*r^2)/2 + 1,          (dt^3*r^3)/6 - dt*r]
[         dt*r - (dt^3*r^3)/6, (dt^4*r^4)/24 - (dt^2*r^2)/2 + 1]

B_kd =
[ sin(dt*r)/r, cos(dt*r)/r]
[-cos(dt*r)/r, sin(dt*r)/r]

4. 随机线性动态模型

随机线性动态模型：
$$\dot{x}(t)=\mathbf{A}\mathbf{x}(t)+{\mathbf{B}}_w\mathbf{w}(t)$$
随机模型和确定模型的唯一区别是输入$\mathbf{u}(t)$($\mathbf{w}(t)$)。 $\mathbf{w}(t)$是白噪声过程，也成为白色随机过程（white stochastic process）。

自相关函数(Auto-correlation function)：
R w w ( τ ) = E { w ( t + τ ) w ( t ) } = ∑ w δ ( τ ) R_{ww}(\tau) = \text{E}\{{\bm w} (t +\tau){\bm w}(t)\}=\sum_{w}\delta(\tau) Rww​(τ)=E{w(t+τ)w(t)}=w∑​δ(τ)
因此有：
$$\mathbf{x}(t_n)=e^{\mathbf{A}(t_n-t_{n-1}))}\mathbf{x}(t_{n-1})+{\int }_{t_{n-1}}^{t_n}{e}^{\mathbf{A}(t_n-t)}{\mathbf{B}}_w\mathbf{w}(t)dt$$

其中， $\mathbf{w}(t)$是随机的， 无法使用零阶保持，甚至是无法积分的（使用标准方法）。所以我们定义一个随机变量作为随机噪声：
$${\mathbf{q}}_n\triangleq {\int }_{t_{n-1}}^{t_n}{e}^{\mathbf{A}(t_n-t)}{\mathbf{B}}_w\mathbf{w}(t)dt$$
然后有
$${\mathbf{x}}_n={\mathbf{F}}_n{\mathbf{x}}_{n-1}+{\mathbf{q}}_n$$
过程噪声的均值和协方差：
E { q n } = 0 C o v { q n } = E { ( q n − E { q n } ) ( q n − E { q n } ) T } = ∬ t n − 1 t n e A ( t n − t ) B w E { w ( t ) w ( τ ) T } B w T ( e A ( t n − τ ) ) T d τ d t = ∫ t n − 1 t n e A ( t n − τ ) B w ∑ w B w T e A T ( t n − τ ) d τ ≜ Q n \begin{aligned}{\rm E}\{ {\bm q}_n\} & = 0 \\ \\ {\rm Cov}\{{\bm q}_n\} &= {\rm E} \{({\bm q}_n - {\rm E}\{{\bm q}_n\})({\bm q}_n -{\rm E}\{{\bm q}_n\})^T\} \\ &= \iint _{t_{n-1}}^{t_n}e^{{\bm A}(t_n - t)}{\bm B}_w {\rm E}\{ {\bm w}(t){\bm w}(\tau)^T\}B^T_w(e^{{\bm A}(t_n - \tau)})^T d\tau dt \\ & =\int _{t_{n-1}}^{t_n}e^{{\bm A}(t_n - \tau)}{\bm B}_w \sum _w{\bm B}_w^T e^{{\bm A}^T(t_n - \tau)}d\tau \\ &\triangleq {\bm Q}_n\end{aligned} E{qn​}Cov{qn​}​=0=E{(qn​−E{qn​})(qn​−E{qn​})T}=∬tn−1​tn​​eA(tn​−t)Bw​E{w(t)w(τ)T}BwT​(eA(tn​−τ))Tdτdt=∫tn−1​tn​​eA(tn​−τ)Bw​w∑​BwT​eAT(tn​−τ)dτ≜Qn​​
其符合正态分布：
$${\mathbf{q}}_n\sim \mathcal{N}(0,{\mathbf{Q}}_n)$$

离散的随机线性动态模型：
$${\mathbf{x}}_n={\mathbf{F}}_n{\mathbf{x}}_{n-1}+{\mathbf{q}}_n$$
其中：
$$\begin{array}{rl}{\mathbf{F}}_n& \triangleq e^{\mathbf{A}(t_n-t_{n-1})}\\ {\mathbf{q}}_n& \sim N(0,{\mathbf{Q}}_n)\\ {\mathbf{Q}}_n& ={\int }_{t_{n-1}}^{t_n}{e}^{\mathbf{A}(t_n-\tau )}{\mathbf{B}}_w\sum _w{\mathbf{B}}_w^T{e}^{{\mathbf{A}}^T(t_n-\tau )}d\tau \end{array}$$

示例：一维维纳速度模型

数学中，维纳过程（英语：Wiener process）是一种连续时间随机过程，得名于诺伯特·维纳。由于与物理学中的布朗运动有密切关系，也常被称为“布朗运动过程”或简称为布朗运动。维纳过程是莱维过程（指左极限右连续的平稳独立增量随机过程）中最有名的一类，在纯数学、应用数学、经济学与物理学中都有重要应用

动态模型：
$$\left[\begin{array}{c}\dot{p}(t)\\ \dot{v}(t)\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\left[\begin{array}{c}p(t)\\ v(t)\end{array}\right]+\left[\begin{array}{c}0\\ 1\end{array}\right]w(t)$$
且有白噪声过程$w(t)$和$R_{ww}(\tau )={\sigma }_w^2\delta (\tau )$。

过程噪声协方差：
$${\mathbf{Q}}_n={\int }_{t_{n-1}}^{t_n}{e}^{\mathbf{A}(t_n-\tau )}{\mathbf{B}}_w\sum _w{\mathbf{B}}_w^T{e}^{{\mathbf{A}}^T(t_n-\tau )}d\tau$$
其中：
$$\begin{array}{rl}{e}^{\mathbf{A}(t_n-\tau )}& =\left[\begin{array}{cc}1& t_n-\tau \\ 0& 1\end{array}\right]\\ e^{\mathbf{A}(t_n-\tau )}{\mathbf{B}}_w& =\left[\begin{array}{cc}1& t_n-\tau \\ 0& 1\end{array}\right]\left[\begin{array}{c}0\\ 1\end{array}\right]=\left[\begin{array}{c}{t}_n-\tau \\ 1\end{array}\right]\end{array}$$
得到离散化的模型：
$$\left[\begin{array}{c}{p}_n\\ v_n\end{array}\right]=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{p}_{n-1}\\ v_{n-1}\end{array}\right]+{\mathbf{q}}_n$$
且有 ${\mathbf{q}}_n\sim N(0,{\mathbf{Q}}_n)$
$${\mathbf{Q}}_n={\sigma }_w^2\left[\begin{array}{cc}\frac{(\Delta t{)}^3}{3}& \frac{(\Delta t{)}^2}{2}\\ \frac{(\Delta t{)}^2}{2}& \Delta t\end{array}\right]$$

5. 总结

线性ODE模型的离散化
线性连续时间动态模型
$$\dot{x}(t)=\mathbf{A}\mathbf{x}(t)+{\mathbf{B}}_{u}u(t)$$
离散动态模型
$${\mathbf{x}}_n={\mathbf{F}}_n{\mathbf{x}}_{n-1}+{\mathbf{L}}_n{\mathbf{u}}_{n-1}$$
其中
$$\begin{array}{rl}{\mathbf{F}}_n& \triangleq e^{\mathbf{A}(t_n-t_{n-1})}\\ {\mathbf{L}}_n& \triangleq {\int }_{t_{n-1}}^{t_n}{e}^{\mathbf{A}(t_n-t)}{\mathbf{B}}_u\mathrm{d}t\end{array}$$

线性SDE模型的离散化
线性连续时间动态模型
$$\dot{x}(t)=\mathbf{A}\mathbf{x}(t)+{\mathbf{B}}_{w}w(t)$$
离散动态模型
$${\mathbf{x}}_n={\mathbf{F}}_n{\mathbf{x}}_{n-1}+{\mathbf{q}}_n,{\mathbf{q}}_n\sim N(0,{\mathbf{Q}}_n)$$
其中
$${\mathbf{Q}}_n={\int }_{t_{n-1}}^{t_n}{e}^{\mathbf{A}(t_n-\tau )}{\mathbf{B}}_w\sum _w{\mathbf{B}}_w^T{e}^{{\mathbf{A}}^T(t_n-\tau )}d\tau$$