从全状态观测器到卡尔曼滤波器（一）

! https://zhuanlan.zhihu.com/p/338269917

从全状态观测器到卡尔曼滤波器（一）

写在开头

一年前听学长提了个很高端的东西，叫卡尔曼滤波器。学习过程中遇到了很多问题，读了很多博客也还是一头雾水。我读的第一篇博客是：https://zhuanlan.zhihu.com/p/39912633，读完以后基本理解了作者的意思，但是还远不具备设计一个卡尔曼滤波器的能力，因为不懂如何设计\(Q 、R、 H\)三个矩阵。于是用 卡尔曼滤波器 实例 应用 为关键词搜了些博客，发现很多人用的都是一维温度测量的例子：状态向量就只有温度一个变量，\(Q、R\)不变最后整个卡尔曼滤波器收敛为一个一阶低通滤波器。看完后大概了解了\(Q、R\)矩阵的含义，但还是对高维情况下\(H\)矩阵的设计一头雾水。最后从Google搜kalman filter application得到的一些材料才解决了我之前的疑惑：https://towardsdatascience.com/kalman-filter-an-algorithm-for-making-sense-from-the-insights-of-various-sensors-fused-together-ddf67597f35e、https://www.cs.cornell.edu/courses/cs4758/2012sp/materials/MI63slides.pdf

看完这些后自己对卡尔曼滤波器也有了一个大致的概念，但对卡尔曼滤波器“最优”的特性与其本身估计状态的原理仍然不是非常明确。这主要是因为缺乏对卡尔曼滤波器原理推导过程的了解，于是乎就去翻了翻教科书中卡尔曼滤波器的推导，但其中大量矩阵运算的定理我是不理解的，再加上我这个人比较愚钝而且很浮躁，所以关于卡尔曼滤波器推导过程的学习最终也就不了了之了。

前些日子在知乎读到了这篇文章：https://zhuanlan.zhihu.com/p/88535121，又回想起自己初学卡尔曼滤波器时候对\(H\)矩阵设计以及卡尔曼滤波器本身原理的一头雾水，故打算以“从全状态观测器到卡尔曼滤波器”为题，从全状态观测器是如何估计不可测信息出发，用观测器增益\(L\)类比卡尔曼增益\(K\)，并最后给出关于卡尔曼滤波器的不严谨推导，希望能给和我初学时一样一头雾水的朋友一点帮助。另外会给出在我写的适用于cortex-M系列内核单片机的基于CMSIS DSP库矩阵运算的卡尔曼滤波器C语言实现。疫情期间废寝忘食开发这个C语言实现，日夜颠倒两三天，修复了各种各样的小bug，投入了很多心血，故希望分享出来帮助更多人：https://github.com/CharlesW1970/kalman-filter-C-implementation

---

0 从一个简单的例子出发

我们现在希望用一个超声波测距传感器测量固定在直线轨道上的小车位置\(x\)，但这个传感器不是完全准确的，其测量噪声服从期望为0的正态分布

\[z(t)={x}(t)+{v}(t), {v}(t) \sim {N}(0, r)\\ \]

其中\(z(t)\)为\(t\)时刻测量值，\(x(t)\)为\(t\)时刻温度，\(v(t)\)为为\(t\)时刻测量噪声。

1 均值滤波

若小车是静止的，我们很自然想到可以通过在一段时间内多此测量取平均值的方法来消除噪声的影响以获得一个准确的估计值\(\hat x\)

\[\hat x = \frac{z(t_0)+z(t_0 +\Delta t)+z(t_0 +2\Delta t)+\cdots+z(t_0 +k\Delta t)}{k+1}\\ \]

根据大数定律，当测量次数\(k\)足够多的时候，测量值中噪声的影响的总和为0的概率为1

\[\lim _{k \rightarrow \infty} P(|\bar{v}-0|<\epsilon)=1\\ \]

也就是说，我们测量的数据越多，测量的时间越长，就越有可能通过取平均值的方法获取没有误差\(e = x-\hat x\)的估计值。

2 状态空间

2.1 系统的状态

但当小车不处于静止状态的时候，长时间测量多组数据取平均值的方法就不再有效了：为了更好的抑制噪声我们需要测量更多数据，但更多的历史数据意味着估计值的滞后越大。因此我们需要优化我们的模型：小车不静止，意味着小车位置\(x\)存在变化，那么我们需要一个可以描述小车位置\(x\)变化的量来丰富我们的模型，即小车的速度\(v = \dot x\)。现在我们拿到了两个量，一个是我们关注的对象位置\(x\)，一个是位置\(x\)的一阶导，设

\[x_1 = x\\ x_2 = \dot x \]

我们很自然的可以将这两个标量组合成一个向量\(\mathbf x\)

\[\mathbf x=\left[\begin{array}{c} x_1 \\ x_2 \end{array}\right]=\left[\begin{array}{c} x \\ \dot x \end{array}\right]\\ \]

我们称向量\(\mathbf x\)为状态向量，用于描述系统或对象的状态。系统的状态在不断变化，而微分方程是描述动态系统变化的好方法，因此我们需要建立一个微分方程用于描述小车状态的变化情况，一般来说我们需要构建形式如下的微分方程

\[\dot {\mathbf x} = A\mathbf x\\ \]

我们称\(A\)为系统矩阵，根据\(x_1 = x,x_2 = \dot x\)这个假设我们很容易确定矩阵\(A\)

\[\dot {\mathbf x} = \left[\begin{array}{c} \dot x_1 \\ \dot x_2 \end{array}\right] =\left[\begin{array}{c} x_2 \\ 0 \end{array}\right]= \left[\begin{array}{c} 0 \quad 1 \\ 0 \quad 0 \end{array}\right] \left[\begin{array}{c} x_1 \\ x_2 \end{array}\right] \\ \]

即

\[A = \left[\begin{array}{c} 0 \quad 1 \\ 0 \quad 0 \end{array}\right]\\ \]

注意这里我们建立的是匀速模型，即\(\dot x_2 = 0\)。若假设匀加速模型则可设置三维状态向量\(\mathbf x = [x \quad \dot x \quad \ddot x]^T\)。如果小车加速度较小，而且我们迭代微分方程的周期\(\Delta t\)足够小，我们就完全可以采用匀速模型，因为更小的维数意味着更少的算式更小的计算量。

2.2 加入控制

如果我们是为反馈控制系统反馈的需要而估计小车的位置，那么我们一定能获取反馈控制器的输出，这样一来我们就可以结合控制器输出更准确的估计状态。根据控制器输出我们可以将向量方程\(\dot {\mathbf x} = A\mathbf x\)扩展为以下形式

\[\dot {\mathbf x} = A\mathbf x +B\mathbf u\\ \]

其中\(B\)为输入矩阵或控制矩阵，而\(\mathbf u\)为输入向量或控制向量。对于单输入系统而已输入向量\(\mathbf u\)为标量，即\(\mathbf u = u\)，而矩阵\(B\)的维数由输入和状态向量的维数共同确定，具体元素值由输入与状态的关系确定。我们假作用在小车上沿直线轨道与小车位置\(x\)具有相同正方向的力\(F\)为控制器的输出，作为输入\(u\)作用于小车系统。根据牛顿第二定律，有

\[u=F = ma = m\ddot x = m\dot x_2\\ \]

即

\[\dot x_2 = \frac{u}{m}\\ \]

根据上式我们可以确定矩阵\(B\)

\[\dot {\mathbf x} = \left[\begin{array}{c} \dot x_1 \\ \dot x_2 \end{array}\right] = \left[\begin{array}{c} 0 \quad 1 \\ 0 \quad 0 \end{array}\right] \left[\begin{array}{c} x_1 \\ x_2 \end{array}\right] + \left[\begin{array}{c} 0 \\ \frac{1}{m} \end{array}\right]u \\ \]

即

\[B = \left[\begin{array}{c} 0 \\ \frac{1}{m} \end{array}\right]\\ \]

2.3 确定输出

状态向量中包含多个量，称其中我们真正关注的对象为系统输出\(\mathbf y\)。其形式如下

\[\mathbf y = C\mathbf x +D\mathbf u\\ \]

其中\(C\)为输出矩阵，为直接传动项，在本系列中我们只关注\(\mathbf y = C\mathbf x\)。与输入向量\(\mathbf u\)类似，对于单输出系统而已输出向量\(\mathbf y\)为标量，即\(\mathbf y = y\)，而对于多输入系统而言，我们可以根据变量是否可测设置系统输出，即谁可测谁作输出的一部分。在小车这个单输入单输出中，我们关注小车位置\(x\)，并且小车位置\(x\)是可测的，因此设置\(x_1\)为系统输出，即可确定输出矩阵\(C\)

\[y = x = x_1 = [1 \quad 0]\left[\begin{array}{c} x_1 \\ x_2 \end{array}\right]\\ \]

即

\[C = [1\quad 0]\\ \]

到这一步我们已经完整表示了这个线性动态系统与被控对象动态，即

\[\dot {\mathbf x} = A\mathbf x +B\mathbf u\\ \mathbf y = C\mathbf x +D\mathbf u\\ \]

其中

\[\mathbf x=\left[\begin{array}{c} x \\ \dot x \end{array}\right] \quad \mathbf u=[F] \quad A = \left[\begin{array}{c} 0 \quad 1 \\ 0 \quad 0 \end{array}\right]\quad B = \left[\begin{array}{c} 0 \\ \frac{1}{m} \end{array}\right]\quad C = [1\quad 0] \quad D = [0]\\ \]

这种描述系统的方法被称为状态空间法，状态空间的思想来源于描述微分方程的状态变量法。在状态变量法中，通过关于系统状态向量的一阶微分方程描述一个动态系统的微分方程组，而方程组的解则可以看作状态向量在空间（相图）中的一条轨迹。状态空间方程与经典控制理论中的传递函数类似，均是用于描述动态系统的手段。相比传递函数，状态空间方程有许多优点，比如对非零初始条件的系统仍能保持简单的表示形式、很容易描述多输入多输出系统等。

3 全阶状态观测器

3.1 用测量值纠正

回到最初的问题，我们需要寻找一种方法来估计状态\(\mathbf x\)。通过状态空间法描述小车这个动态系统后，我们便可以可以设计全阶状态观测器来估计小车的状态。

根据系统的输入\(u\)，我们可以通过向量微分方程\(\dot {\mathbf x} = A\mathbf x +Bu\)计算状态\(\mathbf x\)的变化情况，进而结合系统的初始状态\(\mathbf x_0\)积分得到状态\(\mathbf x\)的估计值\(\hat {\mathbf x}\)。但实际情况是初始状态\(\mathbf x_0\)与模型本身均存在误差，这个误差会随着时间不断累积，导致估计值与真实值的误差越来越大。When in trouble, use feedback. 通过借助反馈消除误差，即将传感器测量值与估计值之间的误差反馈到观测器中，并用误差不断纠正观测器的估计值。

估计状态向量中的位置估计值\(\hat x(t)\)为

\[\hat x(t) = \hat x_1(t) = C\hat {\mathbf x}(t)\\ \]

位置测量值为\(z(t)\)与位置估计值\(\hat x(t)\)之间误差为

\[z(t) -C\hat {\mathbf x}(t)\\ \]

与反馈控制器误差乘增益得到控制器输出类似，估计误差乘一个增益补偿到观测器\(\dot{\hat{\mathbf{x}}}(t) = A\hat{\mathbf{x}}(t) +Bu(t)\)中，有

\[\dot{\hat{\mathbf{x}}}(t) = A\hat{\mathbf{x}}(t) +Bu(t) + L(z(t) -C\hat {\mathbf x}(t))\\ \]

其中\(L\)为观测增益。

3.2 确定观测增益\(L\)

状态观测器中的\(A、 B、 C\)三个矩阵均由系统本身确定，而确定观测增益矩阵\(L\)是设计观测器的核心。观测增益\(L\)的设计目的是使\(t \rightarrow \infty\) 时，\(\hat{\mathbf{x}}(t) \rightarrow \mathbf{x}(t)\)，即估计误差随时间衰减为0。

定义估计误差如下

\[\mathbf e(t) = \mathbf x(t) - \mathbf {\hat x}(t)\\ \]

等号两边对时间\(t\)求导

\[\dot{\mathbf{e}}(t)=\dot{\mathbf{x}}(t)-\dot{\hat{\mathbf{x}}}(t)\\ \]

代入状态观测器公式有

\[\dot{\mathbf{e}}(t)={A} {\mathbf{x}}(t)+{B} u(t)-{A} \hat{\mathbf{x}}(t)-{B} u(t)-{L}(z(t)-{C} \hat{\mathbf{x}}(t))\\ \]

我们最初给测量值\(z(t)\)的定义如下

\[z(t)={x}(t)+{v}(t), {v}(t) \sim {N}(0, r)\\ \]

其中\(x(t)\)为\(t\)时刻温度，\(v(t)\)为为\(t\)时刻测量噪声。这里我们先暂时不考虑测量噪声，即测量绝对准确

\[z(t)={x}(t)=C\mathbf x\\ \]

代入估计误差微分方程中，可将其简化为关于估计误差\(\mathbf e\)形如\(\dot {\mathbf x} = A\mathbf x +B\mathbf u\)的状态空间方程

\[\dot{\mathbf{e}}(t)={A} {\mathbf{x}}(t)-{A} \hat{\mathbf{x}}(t)-{L}C\mathbf x(t)+L{C} \hat{\mathbf{x}}(t)\\ =(A - LC)(\mathbf x(t) - \mathbf {\hat x}) \\= (A - LC)\mathbf e(t) \]

想要找到合适的增益矩阵\(L\)使估计误差渐近稳定，需要先分析这个状态空间描述的动态系统的动态响应。通过拉普拉斯变换将估计误差\(\mathbf e\)的微分方程（状态空间方程）由时域变换至\(s\)域

\[s \mathbf{E}(s)-\mathbf{e}(0)={(A - LC)} \mathbf{E}(s)\\ \]

化简得

\[\mathbf{E}(s) = (sI - (A - LC))^{-1}\mathbf{e}(0) =\frac{(sI - (A - LC))^{*}}{|sI - (A - LC)|}\mathbf{e}(0)\\ \]

显然，\(E(s)\)的极点恰好为矩阵\(A - LC\)的特征值。故观测器的特征方程为

\[|sI - (A - LC)| = 0\\ \]

只要特征方程的根全部位于复平面左半平面，即特征值实部小于0，对于任意初始观测误差\(\mathbf{e}(0)\)，当使\(t \rightarrow \infty\) 时，均有\(\mathbf e(t) \rightarrow 0\)。因此，观测器的设计问题便简化为寻找合适的观测增益\(L\)使特征方程的根全部位于复平面左半平面，即极点配置。

全状态观测器与全状态反馈控制器设计思路基本一致，甚至他们在数学上是等价的。值得注意的是在状态观测器收敛速度一般来说应快于反馈控制器。经验上讲，观测器极点应比控制器极点远2~6倍。但如果传感器噪声很大以至于会影响控制品质，则可以相应减小观测器的收敛速度以抑制噪声。

根据经验配置观测器极点不容易得到状态的最优估计，尤其是在测量噪声或过程噪声（模型误差）较大的情况下。根据最有估计理论，观测器增益的最佳选择取决于测量噪声\(v\)与过程噪声\(w\)的大小关系，而卡尔曼滤波器就是一种根据测量噪声\(v\)与过程噪声\(w\)确定增益的最优观测器，而观测增益确定方法的不同即是卡尔曼滤波器与现代控制理论中全状态观测器最核心的区别（状态观测器通过极点配置设计增益，而卡尔曼滤波器通过结合测量噪声\(v\)与过程噪声\(w\)与状态估计误差的方差确定增益K）。