状态外插方程
我想首先讲解状态外插方程。
使用状态外插方程，我们能够基于当前系统状态预测下一个系统状态。它把当前（\( n \) 时刻）的状态向量外插至未来（\( n + 1 \) 时刻）。
状态外插方程描述系统的动态模型。其他文献中也叫：
- 预测器方程
- 转移方程
- 预测方程
- 动态模型
- 状态方程模型
使用矩阵描述的状态外插方程的一般形式为：
| \( \boldsymbol{\hat{x}}_{n+1,n} \) | 是 \( n \) 时刻对 \( n + 1 \) 时刻系统状态的预测 |
| \( \boldsymbol{\hat{x}}_{n,n} \) | 是 \( n \) 时刻系统状态向量的估计 |
| \( \boldsymbol{u}_{n} \) | 是 控制向量 或 输入向量 - 该系统的一个 可测量的（确定性的）输入 |
| \( \boldsymbol{w}_{n} \) | 是 过程噪声 或 扰动 - 能够影响系统状态的 不可测量的 输入 |
| \( \boldsymbol{F} \) | 是 状态转移矩阵 |
| \( \boldsymbol{G} \) | 是 控制矩阵 或 输入转移矩阵 （将控制量映射到状态变量上） |
下图给出了状态外插方程的原理描述。
状态变量可以表征我们所感兴趣的系统属性。
例如，运动中的车辆具有三个属性：位置、速度和加速度。
你也许会问，哪些属性是状态变量，哪些属性又是系统的输入呢？
- 运动中的机械系统具有诸如位置、速度、加速度和阻力等属性。
- 作用在系统上的力应该被视为外部输入，因为它会改变系统的状态（考虑前文匀加速示例中的位置和速度）。
- 牛顿第二定律表明 \( F = ma \)，因此我们认为加速度是系统的输入。
- 位置和速度则是我们主要关心的状态变量。
对于一个弹簧，施加在弹簧上的力 \( F(t) \) 是输入 \( u(t) \)，弹簧形变 \( x(t) \) 是系统状态。
对于一个自由落体中的物体，输入是重力 \( F_{g} \) 和阻力 \( F_{drag}(t) \)，物体高度 \( h(t) \) 和速度 \( v(t) \) 则是系统状态。
来看看几个状态外插方程的例子：
示例 - 飞机 - 没有控制输入
本例中我们来定义一架匀加速运动的飞机的状态外插方程。
这个例子里没有输入，在下一个例子里会加入输入：
\[ \boldsymbol{u}_{n} = 0 \]
考虑一架在三维空间匀加速运动的飞机。用于在笛卡尔空间 \( \left( x,y,z \right) \) 描述飞机位置、速度和加速度的状态向量 \( \boldsymbol{\hat{x}_{n}} \) 为：
\[ \boldsymbol{\hat{x}}_{n}= \left[ \begin{matrix} \hat{x}_{n}\\ \hat{y}_{n}\\ \hat{z}_{n}\\ \hat{\dot{x}}_{n}\\ \hat{\dot{y}}_{n}\\ \hat{\dot{z}}_{n}\\ \hat{\ddot{x}}_{n}\\ \hat{\ddot{y}}_{n}\\ \hat{\ddot{z}}_{n}\\ \end{matrix} \right] \]
状态转移矩阵 \( \boldsymbol{F} \) 为：
\[ \boldsymbol{F}= \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2} & 0 & 0 \\ 0 & 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2} & 0 \\ 0 & 0 & 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2}\\ 0 & 0 & 0 & 1 & 0 & 0 & \Delta t & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & \Delta t & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & \Delta t\\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \]
状态外插方程为：
\[ \boldsymbol{\hat{x}}_{n+1,n} = \boldsymbol{F\hat{x}}_{n} \]
\[ \left[ \begin{matrix} \hat{x}_{n+1,n}\\ \hat{y}_{n+1,n}\\ \hat{z}_{n+1,n}\\ \hat{\dot{x}}_{n+1,n}\\ \hat{\dot{y}}_{n+1,n}\\ \hat{\dot{z}}_{n+1,n}\\ \hat{\ddot{x}}_{n+1,n}\\ \hat{\ddot{y}}_{n+1,n}\\ \hat{\ddot{z}}_{n+1,n}\\ \end{matrix} \right] = \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2} & 0 & 0 \\ 0 & 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2} & 0 \\ 0 & 0 & 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2}\\ 0 & 0 & 0 & 1 & 0 & 0 & \Delta t & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & \Delta t & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & \Delta t\\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \left[ \begin{matrix} \hat{x}_{n,n}\\ \hat{y}_{n,n}\\ \hat{z}_{n,n}\\ \hat{\dot{x}}_{n,n}\\ \hat{\dot{y}}_{n,n}\\ \hat{\dot{z}}_{n,n}\\ \hat{\ddot{x}}_{n,n}\\ \hat{\ddot{y}}_{n,n}\\ \hat{\ddot{z}}_{n,n}\\ \end{matrix} \right] \]
矩阵乘法结果：
\[ \begin{cases} \hat{x}_{n+1,n} = \hat{x}_{n,n} + \hat{\dot{x}}_{n,n} \Delta t+ \frac{1}{2}\hat{\ddot{x}}_{n,n} \Delta t^{2}\\ \hat{y}_{n+1,n} = \hat{y}_{n,n} + \hat{\dot{y}}_{n,n} \Delta t+ \frac{1}{2}\hat{\ddot{y}}_{n,n} \Delta t^{2}\\ \hat{z}_{n+1,n} = \hat{z}_{n,n} + \hat{\dot{z}}_{n,n} \Delta t+ \frac{1}{2}\hat{\ddot{z}}_{n,n} \Delta t^{2}\\ \hat{\dot{x}}_{n+1,n} = \hat{\dot{x}}_{n,n} + \hat{\ddot{x}}_{n,n} \Delta t\\ \hat{\dot{y}}_{n+1,n} = \hat{\dot{y}}_{n,n} + \hat{\ddot{y}}_{n,n} \Delta t\\ \hat{\dot{z}}_{n+1,n} = \hat{\dot{z}}_{n,n} + \hat{\ddot{z}}_{n,n} \Delta t\\ \hat{\ddot{x}}_{n+1,n} = \hat{\ddot{x}}_{n,n}\\ \hat{\ddot{y}}_{n+1,n} = \hat{\ddot{y}}_{n,n}\\ \hat{\ddot{z}}_{n+1,n} = \hat{\ddot{z}}_{n,n}\\ \end{cases} \]
示例 - 飞机 - 有控制输入
本例和上个示例类似，但现在飞行员的操纵杆上有一个传感器，可以通过它读取飞行员的加速度操纵指令。
用于在笛卡尔坐标系 \( \left( x,y,z \right) \) 里描述飞机位置和速度估计值的状态向量 \( \boldsymbol{\hat{x}_{n}} \) 为：
\[ \boldsymbol{\hat{x}}_{n}= \left[ \begin{matrix} \hat{x}_{n}\\ \hat{y}_{n}\\ \hat{z}_{n}\\ \hat{\dot{x}}_{n}\\ \hat{\dot{y}}_{n}\\ \hat{\dot{z}}_{n}\\ \end{matrix} \right] \]
在笛卡尔坐标系 \( \left( x,y,z \right) \) 里描述测量到的飞机加速度的控制向量 \( \boldsymbol{u_{n}} \) 为：
\[ \boldsymbol{u}_{n}= \left[ \begin{matrix} \ddot{x}_{n}\\ \ddot{y}_{n}\\ \ddot{z}_{n}\\ \end{matrix} \right] \]
状态转移矩阵 \( \boldsymbol{F} \) 为：
\[ \boldsymbol{F}= \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0\\ 0 & 1 & 0 & 0 & \Delta t & 0\\ 0 & 0 & 1 & 0 & 0 & \Delta t\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \]
控制矩阵 \( \boldsymbol{G} \) 为：
\[ \boldsymbol{G}= \left[ \begin{matrix} 0.5\Delta t^{2} & 0 & 0 \\ 0 & 0.5\Delta t^{2} & 0 \\ 0 & 0 & 0.5\Delta t^{2} \\ \Delta t & 0 & 0 \\ 0 & \Delta t & 0 \\ 0 & 0 & \Delta t \\ \end{matrix} \right] \]
状态外插方程为：
\[ \boldsymbol{\hat{x}}_{n+1,n} = \boldsymbol{F\hat{x}}_{n,n} + \boldsymbol{Gu}_{n,n} \]
\[ \left[ \begin{matrix} \hat{x}_{n+1,n}\\ \hat{y}_{n+1,n}\\ \hat{z}_{n+1,n}\\ \hat{\dot{x}}_{n+1,n}\\ \hat{\dot{y}}_{n+1,n}\\ \hat{\dot{z}}_{n+1,n}\\ \end{matrix} \right] = \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0\\ 0 & 1 & 0 & 0 & \Delta t & 0\\ 0 & 0 & 1 & 0 & 0 & \Delta t\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \left[ \begin{matrix} \hat{x}_{n,n}\\ \hat{y}_{n,n}\\ \hat{z}_{n,n}\\ \hat{\dot{x}}_{n,n}\\ \hat{\dot{y}}_{n,n}\\ \hat{\dot{z}}_{n,n}\\ \end{matrix} \right] + \left[ \begin{matrix} 0.5\Delta t^{2} & 0 & 0 \\ 0 & 0.5\Delta t^{2} & 0 \\ 0 & 0 & 0.5\Delta t^{2} \\ \Delta t & 0 & 0 \\ 0 & \Delta t & 0 \\ 0 & 0 & \Delta t \\ \end{matrix} \right] \left[ \begin{matrix} \ddot{x}_{n}\\ \ddot{y}_{n}\\ \ddot{z}_{n}\\ \end{matrix} \right] \]
示例 – 自由落体
考虑一个自由落体中的物体，状态向量包括高度 \( h \) 和物体速度 \( \dot{h} \):
\[ \boldsymbol{\hat{x}}_{n}= \left[ \begin{matrix} \hat{h}_{n}\\ \hat{\dot{h}}_{n}\\ \end{matrix} \right] \]
状态转移矩阵 \( \boldsymbol{F} \) 为：
\[ \boldsymbol{F}= \left[ \begin{matrix} 1 & \Delta t \\ 0 & 1 \\ \end{matrix} \right] \]
控制矩阵 \( \boldsymbol{G} \) 为：
\[ \boldsymbol{G}= \left[ \begin{matrix} 0.5\Delta t^{2} \\ \Delta t \\ \end{matrix} \right] \]
输入向量 \( \boldsymbol{u}_{n} \) 为：
\[ \boldsymbol{u}_{n}= \left[ \begin{matrix} g \end{matrix} \right] \]
式中 \( g \) 是重力加速度。
我们并没有感知加速度的传感器，但我们根据定律知道对于自由落体的物体，其加速度等于 ( g \).
状态外插方程是下面这样：
\[ \left[ \begin{matrix} \hat{h}_{n+1,n}\\ \hat{\dot{h}}_{n+1,n}\\ \end{matrix} \right] = \left[ \begin{matrix} 1 & \Delta t \\ 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} \hat{h}_{n,n}\\ \hat{\dot{h}}_{n,n}\\ \end{matrix} \right] + \left[ \begin{matrix} 0.5\Delta t^{2} \\ \Delta t \\ \end{matrix} \right] \left[ \begin{matrix} g \end{matrix} \right] \]
矩阵乘法结果是：
\[ \begin{cases} \hat{h}_{n+1,n} = \hat{h}_{n,n} + \hat{\dot{h}}_{n,n} \Delta t + 0.5 \Delta t^{2} g\\ \hat{\dot{h}}_{n+1,n} = \hat{\dot{h}}_{n,n} + \Delta t g\\ \end{cases} \]
状态外插方程的维度
下表明确了状态外插方程中各个量的维度：
| 量 | 描述 | 维度 |
|---|---|---|
| \( \boldsymbol{x} \) | 状态向量 | \( n_{x} \times 1 \) |
| \( \boldsymbol{F} \) | 状态转移矩阵 | \( n_{x} \times n_{x} \) |
| \( \boldsymbol{u} \) | 输入向量 | \( n_{u} \times 1 \) |
| \( \boldsymbol{G} \) | 控制矩阵 | \( n_{x} \times n_{u} \) |
| \( \boldsymbol{w} \) | 过程噪声向量 | \( n_{x} \times 1 \) |
线性时不变系统
线性卡尔曼滤波假设系统模型是 LTI（线性时不变） 的。
那么什么是“线性”，什么又是“时不变”呢？
线性系统 的所有系统方程中，变量永远不会互相乘在一起，它们只会和常量相乘，然后加在一起。线性系统用来描述变量之间的静态和动态关系。
所谓线性系统，就是一个输出函数 \( \mathcal{F} \) 满足下述方程的系统：
\[ \mathcal{F} \left( a \times g \left( t \right) +b \times h \left( t \right) \right) = a \times \mathcal{F} \left( g \left( t \right) \right) + b \times \mathcal{F} \left( h \left( t \right) \right) \]
式中：
\( a \) 和 \( b \) 是常值实数
\( g \) 和 \( h \) 是任何具有独立变量 \( t \) 的函数
线性系统具有两个基本性质：
- 你可以把函数里变量前的常量系数（上面的 \( a \) 和 \( b \)）“提”到函数外面来（齐次性）。
- 系统对一系列输入的和的响应等于系统对各自输入响应的和（叠加性）。
一个 时不变 系统的 系统函数 不直接包含（显含）时间。
考虑一个增益为 \( G = 10 \) 的放大器。
这个系统是时不变的，尽管系统的输出会随着时间改变，但是系统函数本身并不是随时间改变的。
一个时不变系统的输入如果发生了时延（或时移），它的输出也会发生同样的时延或时移。