状态外插方程

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

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

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

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

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

\[ \boldsymbol{\hat{x}}_{n+1,n} = \boldsymbol{F\hat{x}}_{n,n} + \boldsymbol{Gu}_{n} + \boldsymbol{w}_{n} \]
式中:
\( \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 \) 有时也用希腊字母 \( \Phi \) 来表示。

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

Kalman Filter Extrapolation

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

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

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

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

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

Spring System

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

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

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

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

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

这个例子里没有输入,在下一个例子里会加入输入:

\[ \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{\hat{x}_{n}} \) (粗体)和飞机位置 \( x \) 的估计 \( \hat{x}_{n} \) (非粗体)。本教程中两个符号的字母都是x,仅是字体不同。

状态转移矩阵 \( \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 \) 的函数

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

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

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

考虑一个增益为 \( G = 10 \) 的放大器。

Amplifier

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

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

上一章 下一章