从全状态观测器到卡尔曼滤波器（二）
写在开头
在（一）中通过一个简单的例子介绍了设计连续时间全状态观测器的基本思路，链接：https://zhuanlan.zhihu.com/p/338269917，通过数字计算平台实现状态观测器需要其离散时间形式，本篇将沿用（一）中的例子介绍全状态观测器的离散实现与MIMO推广。
0 回顾（一）中的例子
我们现在希望用一个超声波测距传感器测量固定在直线轨道上的小车位置\(x\)，但这个传感器不是完全准确的，其测量噪声服从期望为0的正态分布
\[z(t)={x}(t)+{v}(t), {v}(t) \sim {N}(0, r)\\ \]
其中\(z(t)\)为\(t\)时刻测量值，\(x(t)\)为\(t\)时刻温度，\(v(t)\)为为\(t\)时刻测量噪声。实际上，状态观测器除了能减小测量噪声对最终结果的影响，更重要的应用是估计不可测的状态量，比如这个例子中的速度\(\dot x\)。
在上一篇中我们设计了连续时间形式的全状态观测器用于估计小车的真实位置
\[\dot{\hat{\mathbf{x}}}(t) = A\hat{\mathbf{x}}(t) +Bu(t) + L(z(t) -C\hat {\mathbf x}(t))\\ \]
其中
\[\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 L = \left[\begin{array}{c} l_1 \\ l_2 \end{array}\right]\\ \]
1 观测器的离散化
1.1 欧拉方法
欧拉方法是一种解决数值常微分方程的最基本的一类显型方法，以以下微分方程为例，已知
\[y^{\prime}(t)=f(t, y(t)), \quad y\left(t_{0}\right)=y_{0}\\ \]
可通过\(y\)在某点附近的线性近似求得其近似解，利用\(t_k\)时刻的数值，采用单步欧拉方法可得到\(t_{k+1} = t_k+\Delta t\)时刻的近似解
\[y_{k+1}=y_{k}+\Delta t f\left(t_{k}, y_{k}\right)\\ \]
1.2 离散化
我们通过欧拉方法得到状态观测器的递推方程，即将观测器由连续时间形式的微分方程转为离散时间形式的差分方程
\[\hat{\mathbf{x}}_{k+1} = \hat{\mathbf{x}}_k + \Delta t\dot{\hat{\mathbf{x}}}_k\\ \]
将\(\dot{\hat{\mathbf{x}}}(t) = A\hat{\mathbf{x}}(t) +Bu(t) + L(z(t) -C\hat {\mathbf x}(t))\)代入
\[\hat{\mathbf{x}}_{k+1} = \hat{\mathbf{x}}_k + \Delta t(A\hat{\mathbf{x}}_k +Bu_k + L(z_k -C\hat{\mathbf{x}}_k))\\ \]
化简得
\[\hat{\mathbf{x}}_{k+1} = F\hat{\mathbf{x}}_k +B^-u_k + K(z_k -C\hat{\mathbf{x}}_k)\\ \]
其中
\[F = I+\Delta t A =\left[\begin{array}{c} 1 \quad \Delta t \\ 0 \quad 1 \end{array}\right]\quad B^- = \Delta t B =\left[\begin{array}{c} 0 \\ \frac{\Delta t}{m} \end{array}\right]\quad K = \Delta t L =\left[\begin{array}{c} \Delta tl_1 \\ \Delta tl_2 \end{array}\right]\\ \]
1.3 反馈的滞后
上面的离散形式存在一个严重的问题，即测量值滞后于估计值一个周期，因为我们计算\(k+1\)时刻的估计值时使用的是\(k\)时刻的反馈\(z_k -C\hat{\mathbf{x}}_k\)。为解决反馈的滞后问题，需要将上面的单个差分方程拆分成两个：预测与修正
预测以得到先验估计\(\hat{\mathbf{x}}_{k+1|k}\)
\[\hat{\mathbf{x}}_{k+1|k} = F\hat{\mathbf{x}}_{k|k} +B^-u_k\\ \]
测量值参与修正以得到后验估计\(\hat{\mathbf{x}}_{k+1|k+1}\)
\[\hat{\mathbf{x}}_{k+1|k+1} = \hat{\mathbf{x}}_{k+1|k} +K(z_{k+1} - C\hat{\mathbf{x}}_{k+1|k})\\ \]
这样一来，我们就得到了全状态观测器完整的离散形式，即先预测、后修正。
1.4 步长的选取
离散状态观测器在数字计算平台中的更新频率即步长的选取不应过长也不应过短。
步长过短会增大单位时间内计算设备的计算量，在计算设备算力允许的情况下一般不会有太恶劣的影响。
但步长过长则会导致模型误差增大从而严重影响预测精度。以匀速模型为例，步长越长则周期内系统加速度的值或速度的变化就越可能不符合模型的匀速假设。若测量值更新缓慢，如视觉目标跟踪中预测目标位置设置ROI的场合，目标位置的测量更新频率受制于摄像头帧率与算法计算速度。可通过提高观测器阶数，即由匀速模型扩展为匀加速模型以提高预测精度。
2 直观理解
对初学者而言，尽管明白状态观测器的数学原理，其矩阵的表达形式也难免会让初学者感到云里雾里。接下来我们通过将上面例子的状态观测器中矩阵与向量乘开，稍加变形得到更直观的表达形式，以观察这个观测器是如何得到位置与速度估计的。
2.1 计算标量代数式
简单起见，我们假设观测期间系统无输入，即\(u=0\)，有
\[\hat{\mathbf{x}}_{k+1|k} = F\hat{\mathbf{x}}_k\\ \]
\[\hat{\mathbf{x}}_{k+1|k+1} = \hat{\mathbf{x}}_{k+1|k} +K(z_{k+1}-C\hat{\mathbf{x}}_{k+1|k})\\ \]
将矩阵\(F\)代入预测
\[\hat{\mathbf{x}}_{k+1|k} = \left[\begin{array}{c} \hat x_{k+1|k} \\ \hat {\dot x}_{k+1|k} \end{array}\right] = \left[\begin{array}{c} 1 \quad \Delta t \\ 0 \quad 1 \end{array}\right] \left[\begin{array}{c} \hat x_{k|k} \\ \hat {\dot x}_{k|k} \end{array}\right]\\ \]
得到两个状态量的先验估计
\[\hat x_{k+1|k} =\hat x_{k|k} + \Delta t\hat {\dot x}_{k|k}\\ \hat {\dot x}_{k+1|k} = \hat {\dot x}_{k|k}\\ \]
设\(k_1 = \Delta tl_1,k_2 = \Delta tl_2\)，将矩阵\(K,C\)代入修正
\[\hat{\mathbf{x}}_{k+1|k} = \left[\begin{array}{c} \hat x_{k+1|k+1} \\ \hat {\dot x}_{k+1|k+1} \end{array}\right] = \left[\begin{array}{c} \hat x_{k+1|k} \\ \hat {\dot x}_{k+1|k} \end{array}\right]+ \left[\begin{array}{c} k_1 \\ k_2 \end{array}\right](z_{k+1} - [1 \quad0] \left[\begin{array}{c} \hat x_{k+1|k} \\ \hat {\dot x}_{k+1|k} \end{array}\right] )\\ \]
得到两个状态量的后验估计
\[\hat x_{k+1|k+1} =\hat x_{k+1|k} + k_1(z_{k+1} - \hat x_{k+1|k})\\ \hat {\dot x}_{k+1|k+1} = \hat {\dot x}_{k+1|k} - k_2(z_{k+1} - \hat x_{k+1|k})\\ \]
2.2 位置估计
我们观察位置的估计\(\hat x_{k+1|k+1}\)，对其变形，有
\[\hat x_{k+1|k+1} =(1-k_1)\hat x_{k+1|k} + k_1z_{k+1}\\ \]
我们发现这是一个典型的互补滤波，对根据模型计算的预测值与通过传感器测得的测量值进行互补滤波
2.3 速度估计
再观察速度的估计值\(\hat {\dot x}_{k+1|k+1}\)，我们发现无法像位置估计表达式那样直接变形，因为等式右侧没有任何同类项。因此我们将上面得到的先验估计表达式\(\hat x_{k+1|k} =\hat x_{k|k} + \Delta t\hat {\dot x}_{k|k}， \hat {\dot x}_{k+1|k} = \hat {\dot x}_{k|k}\)代入，有
\[\hat {\dot x}_{k+1|k+1} = \hat {\dot x}_{k|k} - k_2(z_{k+1} - \hat x_{k|k} + \Delta t\hat {\dot x}_{k|k})\\ \]
合并同类项得
\[\hat {\dot x}_{k+1|k+1} = (1-\Delta tk_2)\hat {\dot x}_{k|k} - k_2(z_{k+1} - \hat x_{k|k})\\ \]
合并后我们并未得到如位置估计值\(\hat x_{k+1|k+1} =(1-k_1)\hat x_{k+1|k} + k_1z_{k+1}\)那样显然的互补形式。我们回顾离散时间域中计算信号一阶导数的常用方法——后向差分：
\[\dot x_k = \frac{x_k - x_{k-1}}{\Delta t}\\ \]
我们发现式\(\hat {\dot x}_{k+1|k+1} = (1-\Delta tk_2)\hat {\dot x}_{k|k} - k_2(z_{k+1} - \hat x_{k|k})\)中\(z_{k+1} - \hat x_{k|k}\)恰好具有后向差分分子的形式，故做如下变形
\[z_{k+1} - \hat x_{k|k} = \Delta t\frac {z_{k+1} - \hat x_{k|k}}{\Delta t}\\ \]
代入速度估计表达式中，有
\[\hat {\dot x}_{k+1|k+1} = (1-\Delta tk_2)\hat {\dot x}_{k|k} - \Delta tk_2\frac {z_{k+1} - \hat x_{k|k}}{\Delta t}\\ \]
得到信号一阶差分形式的同时，也恰好将等式右侧配置为了互补形式，即对信号后向差分得到的一阶导做一阶低通滤波。
3 扩展到MIMO系统
通过将状态观测器展开成标量代数表达式，我们发现状态观测器并没有想象中的那么玄学。有了对状态观测器原理的一定直观理解，我们将上面一维的例子扩展到二维，即通过两个传感器分别测量小车在平面中的\(x,y\)位置，系统输入为分别施加在小车沿\(x,y\)方向上的力，设计状态观测器以获取小车在平面中的坐标与速度。
3.1 观测器动态
回顾一维问题中的状态向量\(\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]\\ \]
扩展到二维直接添加\(y,\dot y\)即可
\[\mathbf x=\left[\begin{array}{c} x_1 \\ x_2 \\ x_3 \\ x_4 \end{array}\right]=\left[\begin{array}{c} x \\ y \\ \dot x\\ \dot y \end{array}\right]\\ \]
根据运动学公式，有
\[\left[\begin{array}{c} \hat x_{k+1|k} \\ \hat y_{k+1|k} \\ \hat {\dot x}_{k+1|k}\\ \hat {\dot y}_{k+1|k}\\ \end{array}\right] = \left[\begin{array}{c} 1 \quad 0 \quad \Delta t \quad0\\ 0 \quad 1 \quad 0\quad \Delta t \\ 0 \quad 0 \quad 1\quad 0\\ 0 \quad 0 \quad 0\quad 1 \end{array}\right] \left[\begin{array}{c} \hat x_{k|k} \\ \hat y_{k|k} \\ \hat {\dot x}_{k|k}\\ \hat {\dot y}_{k|k}\\ \end{array}\right]\\ \]
则状态转移矩阵\(F\)为
\[F = \left[\begin{array}{c} 1 \quad 0 \quad \Delta t \quad0\\ 0 \quad 1 \quad 0\quad \Delta t \\ 0 \quad 0 \quad 1\quad 0\\ 0 \quad 0 \quad 0\quad 1 \end{array}\right]\\ \]
根据牛顿第二定律，有
\[\hat {\dot x}_{k+1|k} = \hat {\dot x}_{k|k} + \Delta t \frac{F_x}{m}\\ \hat {\dot y}_{k+1|k} = \hat {\dot y}_{k|k} + \Delta t \frac{F_y}{m}\\ \]
设系统输入向量为
\[\mathbf u =\left[\begin{array}{c} F_x \\ F_y \end{array}\right]\\ \]
则控制矩阵\(B\)为
\[B =\left[\begin{array}{c} 0 \quad 0\\ 0 \quad 0\\ \frac{\Delta t}{m} \quad 0\\ 0 \quad \frac{\Delta t}{m} \end{array}\right]\\ \]
考虑预测方程的标准形式
\[\hat{\mathbf{x}}_{k+1|k} = F\hat{\mathbf{x}}_{k|k} +B\mathbf u_k\\ \]
将有关向量和矩阵代入，得到离散时间状态观测器的预测方程
\[\hat{\mathbf{x}}_{k+1|k} = \left[\begin{array}{c} \hat x_{k+1|k} \\ \hat y_{k+1|k} \\ \hat {\dot x}_{k+1|k}\\ \hat {\dot y}_{k+1|k}\\ \end{array}\right] = \left[\begin{array}{c} 1 \quad 0 \quad \Delta t \quad0\\ 0 \quad 1 \quad 0\quad \Delta t \\ 0 \quad 0 \quad 1\quad 0\\ 0 \quad 0 \quad 0\quad 1 \end{array}\right] \left[\begin{array}{c} \hat x_{k|k} \\ \hat y_{k|k} \\ \hat {\dot x}_{k|k}\\ \hat {\dot y}_{k|k}\\ \end{array}\right] + \left[\begin{array}{c} 0 \quad 0\\ 0 \quad 0\\ \frac{\Delta t}{m} \quad 0\\ 0 \quad \frac{\Delta t}{m} \end{array}\right] \left[\begin{array}{c} F_x \\ F_y \end{array}\right]\\ \]
3.2 观测器输出
考虑到我们有两个传感器分别测量小车在平面中的\(x,y\)位置，故设观测器输出为
\[\mathbf {\hat y_k} = \left[\begin{array}{c} \hat x_{k|k} \\ \hat y_{k|k} \\ \end{array}\right]= \left[\begin{array}{c} 1 \quad 0 \quad 0 \quad 0\\ 0 \quad 1 \quad 0\quad 0 \\ \end{array}\right]\left[\begin{array}{c} \hat x_{k|k} \\ \hat y_{k|k} \\ \hat {\dot x}_{k|k}\\ \hat {\dot y}_{k|k}\\ \end{array}\right]\\ \]
即输出矩阵\(C\)为
\[C = \left[\begin{array}{c} 1 \quad 0 \quad 0 \quad 0\\ 0 \quad 1 \quad 0\quad 0 \\ \end{array}\right]\\ \]
3.3 完整形式
将传感器测量值加入修正过程即可得到状态观测器的完整形式，这里我们仍不考虑噪声，根据观测器输出的定义\(\mathbf {\hat y_k} = [\hat x_{k|k} \quad \hat y_{k|k} ]^T\)，设测量向量\(z_k\)为
\[z_k = \left[\begin{array}{c} x_{k} \\ y_{k} \\ \end{array}\right]\\ \]
考虑修正方程标准形式
\[\hat{\mathbf{x}}_{k+1|k+1} = \hat{\mathbf{x}}_{k+1|k} +K(z_{k+1} - C\hat{\mathbf{x}}_{k+1|k})\\ \]
将有关向量和矩阵代入，得到离散时间状态观测器的修正方程
\[\hat{\mathbf{x}}_{k+1|k+1} = \left[\begin{array}{c} \hat x_{k+1|k+1} \\ \hat y_{k+1|k+1} \\ \hat {\dot x}_{k+1|k+1}\\ \hat {\dot y}_{k+1|k+1}\\ \end{array}\right] = \left[\begin{array}{c} \hat x_{k+1|k} \\ \hat y_{k+1|k} \\ \hat {\dot x}_{k+1|k}\\ \hat {\dot y}_{k+1|k}\\ \end{array}\right] + \left[\begin{array}{c} k_{11} \quad k_{12}\\ k_{21} \quad k_{22}\\ k_{31} \quad k_{32}\\ k_{41} \quad k_{42} \end{array}\right] (\left[\begin{array}{c} x_{k} \\ y_{k} \\ \end{array}\right] - \left[\begin{array}{c} 1 \quad 0 \quad 0 \quad 0\\ 0 \quad 1 \quad 0\quad 0 \\ \end{array}\right]\left[\begin{array}{c} \hat x_{k+1|k} \\ \hat y_{k+1|k} \\ \hat {\dot x}_{k+1|k}\\ \hat {\dot y}_{k+1|k}\\ \end{array}\right])\\ \]
最后再通过极点配置确定\(K\)矩阵的值即可完成状态观测器的设计。
