状态外插方程

我想首先讲解状态外插方程。

使用状态外插方程，我们能够基于当前系统状态预测下一个系统状态。它把当前（ $n$ 时刻）的状态向量外插至未来（ $n + 1$ 时刻）。

状态外插方程描述系统的动态模型。其他文献中也叫：

预测器方程
转移方程
预测方程
动态模型
状态方程模型

使用矩阵描述的状态外插方程的一般形式为：

{\hat{x}}_{n + 1, n} = {F \hat{x}}_{n, n} + {G u}_{n} + w_{n}

式中：

${\hat{x}}_{n + 1, n}$	是 $n$ 时刻对 $n + 1$ 时刻系统状态的预测
${\hat{x}}_{n, n}$	是 $n$ 时刻系统状态向量的估计
$u_{n}$	是控制向量或输入向量 - 该系统的一个可测量的（确定性的）输入
$w_{n}$	是过程噪声或扰动 - 能够影响系统状态的不可测量的输入
$F$	是状态转移矩阵
$G$	是控制矩阵或输入转移矩阵（将控制量映射到状态变量上）

注：本文中，状态转移矩阵

F

有时也用希腊字母

Φ

来表示。

下图给出了状态外插方程的原理描述。

状态变量可以表征我们所感兴趣的系统属性。

例如，运动中的车辆具有三个属性：位置、速度和加速度。

你也许会问，哪些属性是状态变量，哪些属性又是系统的输入呢？

运动中的机械系统具有诸如位置、速度、加速度和阻力等属性。
作用在系统上的力应该被视为外部输入，因为它会改变系统的状态（考虑前文匀加速示例中的位置和速度）。
牛顿第二定律表明 $F = m a$ ，因此我们认为加速度是系统的输入。
位置和速度则是我们主要关心的状态变量。

对于一个弹簧，施加在弹簧上的力 $F (t)$ 是输入 $u (t)$ ，弹簧形变 $x (t)$ 是系统状态。

对于一个自由落体中的物体，输入是重力 $F_{g}$ 和阻力 $F_{d r a g} (t)$ ，物体高度 $h (t)$ 和速度 $v (t)$ 则是系统状态。

注：过程噪声不直接出现在上述方程中。他们的作用主要是用来描述协方差外插方程中的不确定性，而非用来计算状态转移。

来看看几个状态外插方程的例子：

示例 - 飞机 - 没有控制输入

本例中我们来定义一架匀加速运动的飞机的状态外插方程。

这个例子里没有输入，在下一个例子里会加入输入：

u_{n} = 0

考虑一架在三维空间匀加速运动的飞机。用于在笛卡尔空间 $(x, y, z)$ 描述飞机位置、速度和加速度的状态向量 ${\hat{x}}_{n}$ 为：

{\hat{x}}_{n} = [\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}]

注：请不要混淆了状态向量的估计

{\hat{x}}_{n}

（粗体）和飞机位置

x

的估计

{\hat{x}}_{n}

（非粗体）。本教程中两个符号的字母都是x，仅是字体不同。

状态转移矩阵 $F$ 为：

F = [\begin{matrix} 1 & 0 & 0 & Δ t & 0 & 0 & 0.5 Δ t^{2} & 0 & 0 \\ 0 & 1 & 0 & 0 & Δ t & 0 & 0 & 0.5 Δ t^{2} & 0 \\ 0 & 0 & 1 & 0 & 0 & Δ t & 0 & 0 & 0.5 Δ t^{2} \\ 0 & 0 & 0 & 1 & 0 & 0 & Δ t & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & Δ t & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & Δ 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}]

状态外插方程为：

{\hat{x}}_{n + 1, n} = {F \hat{x}}_{n}

[\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}] = [\begin{matrix} 1 & 0 & 0 & Δ t & 0 & 0 & 0.5 Δ t^{2} & 0 & 0 \\ 0 & 1 & 0 & 0 & Δ t & 0 & 0 & 0.5 Δ t^{2} & 0 \\ 0 & 0 & 1 & 0 & 0 & Δ t & 0 & 0 & 0.5 Δ t^{2} \\ 0 & 0 & 0 & 1 & 0 & 0 & Δ t & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & Δ t & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & Δ 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}] [\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}]

矩阵乘法结果：

{\begin{cases} {\hat{x}}_{n + 1, n} = {\hat{x}}_{n, n} + {\hat{\dot{x}}}_{n, n} Δ t + \frac{1}{2} {\hat{\ddot{x}}}_{n, n} Δ t^{2} \\ {\hat{y}}_{n + 1, n} = {\hat{y}}_{n, n} + {\hat{\dot{y}}}_{n, n} Δ t + \frac{1}{2} {\hat{\ddot{y}}}_{n, n} Δ t^{2} \\ {\hat{z}}_{n + 1, n} = {\hat{z}}_{n, n} + {\hat{\dot{z}}}_{n, n} Δ t + \frac{1}{2} {\hat{\ddot{z}}}_{n, n} Δ t^{2} \\ {\hat{\dot{x}}}_{n + 1, n} = {\hat{\dot{x}}}_{n, n} + {\hat{\ddot{x}}}_{n, n} Δ t \\ {\hat{\dot{y}}}_{n + 1, n} = {\hat{\dot{y}}}_{n, n} + {\hat{\ddot{y}}}_{n, n} Δ t \\ {\hat{\dot{z}}}_{n + 1, n} = {\hat{\dot{z}}}_{n, n} + {\hat{\ddot{z}}}_{n, n} Δ 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}

示例 - 飞机 - 有控制输入

本例和上个示例类似，但现在飞行员的操纵杆上有一个传感器，可以通过它读取飞行员的加速度操纵指令。

用于在笛卡尔坐标系 $(x, y, z)$ 里描述飞机位置和速度估计值的状态向量 ${\hat{x}}_{n}$ 为：

{\hat{x}}_{n} = [\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}]

在笛卡尔坐标系 $(x, y, z)$ 里描述测量到的飞机加速度的控制向量 $u_{n}$ 为：

u_{n} = [\begin{matrix} {\ddot{x}}_{n} \\ {\ddot{y}}_{n} \\ {\ddot{z}}_{n} \end{matrix}]

状态转移矩阵 $F$ 为：

F = [\begin{matrix} 1 & 0 & 0 & Δ t & 0 & 0 \\ 0 & 1 & 0 & 0 & Δ t & 0 \\ 0 & 0 & 1 & 0 & 0 & Δ t \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}]

控制矩阵 $G$ 为：

G = [\begin{matrix} 0.5 Δ t^{2} & 0 & 0 \\ 0 & 0.5 Δ t^{2} & 0 \\ 0 & 0 & 0.5 Δ t^{2} \\ Δ t & 0 & 0 \\ 0 & Δ t & 0 \\ 0 & 0 & Δ t \end{matrix}]

状态外插方程为：

{\hat{x}}_{n + 1, n} = {F \hat{x}}_{n, n} + {G u}_{n, n}

[\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}] = [\begin{matrix} 1 & 0 & 0 & Δ t & 0 & 0 \\ 0 & 1 & 0 & 0 & Δ t & 0 \\ 0 & 0 & 1 & 0 & 0 & Δ t \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}] [\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}] + [\begin{matrix} 0.5 Δ t^{2} & 0 & 0 \\ 0 & 0.5 Δ t^{2} & 0 \\ 0 & 0 & 0.5 Δ t^{2} \\ Δ t & 0 & 0 \\ 0 & Δ t & 0 \\ 0 & 0 & Δ t \end{matrix}] [\begin{matrix} {\ddot{x}}_{n} \\ {\ddot{y}}_{n} \\ {\ddot{z}}_{n} \end{matrix}]

示例 – 自由落体

考虑一个自由落体中的物体，状态向量包括高度 $h$ 和物体速度 $\dot{h}$ :

{\hat{x}}_{n} = [\begin{matrix} {\hat{h}}_{n} \\ {\hat{\dot{h}}}_{n} \end{matrix}]

状态转移矩阵 $F$ 为：

F = [\begin{matrix} 1 & Δ t \\ 0 & 1 \end{matrix}]

控制矩阵 $G$ 为：

G = [\begin{matrix} 0.5 Δ t^{2} \\ Δ t \end{matrix}]

输入向量 $u_{n}$ 为：

u_{n} = [\begin{matrix} g \end{matrix}]

式中 $g$ 是重力加速度。

我们并没有感知加速度的传感器，但我们根据定律知道对于自由落体的物体，其加速度等于 ( g \).

状态外插方程是下面这样：

[\begin{matrix} {\hat{h}}_{n + 1, n} \\ {\hat{\dot{h}}}_{n + 1, n} \end{matrix}] = [\begin{matrix} 1 & Δ t \\ 0 & 1 \end{matrix}] [\begin{matrix} {\hat{h}}_{n, n} \\ {\hat{\dot{h}}}_{n, n} \end{matrix}] + [\begin{matrix} 0.5 Δ t^{2} \\ Δ t \end{matrix}] [\begin{matrix} g \end{matrix}]

矩阵乘法结果是：

{\begin{cases} {\hat{h}}_{n + 1, n} = {\hat{h}}_{n, n} + {\hat{\dot{h}}}_{n, n} Δ t + 0.5 Δ t^{2} g \\ {\hat{\dot{h}}}_{n + 1, n} = {\hat{\dot{h}}}_{n, n} + Δ t g \end{cases}

状态外插方程的维度

下表明确了状态外插方程中各个量的维度：

量	描述	维度
$x$	状态向量	$n_{x} \times 1$
$F$	状态转移矩阵	$n_{x} \times n_{x}$
$u$	输入向量	$n_{u} \times 1$
$G$	控制矩阵	$n_{x} \times n_{u}$
$w$	过程噪声向量	$n_{x} \times 1$

线性时不变系统

线性卡尔曼滤波假设系统模型是 LTI（线性时不变）的。

那么什么是“线性”，什么又是“时不变”呢？

线性系统的所有系统方程中，变量永远不会互相乘在一起，它们只会和常量相乘，然后加在一起。线性系统用来描述变量之间的静态和动态关系。

所谓线性系统，就是一个输出函数 $F$ 满足下述方程的系统：

F (a \times g (t) + b \times h (t)) = a \times F (g (t)) + b \times F (h (t))

式中：

$a$ 和 $b$ 是常值实数

$g$ 和 $h$ 是任何具有独立变量 $t$ 的函数

获取本书

线性系统具有两个基本性质：

你可以把函数里变量前的常量系数（上面的 $a$ 和 $b$ ）“提”到函数外面来（齐次性）。
系统对一系列输入的和的响应等于系统对各自输入响应的和（叠加性）。

一个时不变系统的系统函数不直接包含（显含）时间。

考虑一个增益为 $G = 10$ 的放大器。

这个系统是时不变的，尽管系统的输出会随着时间改变，但是系统函数本身并不是随时间改变的。

一个时不变系统的输入如果发生了时延（或时移），它的输出也会发生同样的时延或时移。