扩展卡尔曼滤波器（二）

指路前一篇扩展卡尔曼滤波器（一）-CSDN博客

考虑更现实的模型

之前的方程：

$$x_k = a * x_{k-1}$$

$$z_k = x_k + v_k$$

实际上这两个方程已经能够适用于很多系统，但有时候需要更加贴合实际情况的描述方程。

在上述方程的基础上，加入控制信号。比如，在第一条式子中， $x_k$被映射为飞机在k时刻的高度，而第一条式子的意义即，飞机在快速降落，飞机高度正以指数速度递减。

而在第一条飞机的运动方程加入控制信号，得到如下式子：

$$x_k = a * x_{k-1} + b * u_k$$

其中， $u_k$为k时刻的控制信号，它也由一个常量 $b$缩放。

通常传感器直接测量数据与它最终表征的系统中的变量（比如此例中为飞机高度），之间为线性关系，他们之间也有一个系数，我们将这一点在我们的方程中体现出来。即传感器的 $r$实际上用来代表直接测量量的误差。得

$$z_k = c * x_k + v_k$$

由此，我们根据实际情况对上述两条方程进行了修正，使其更贴近物理现实。

这里总结列出修改后的两条方程，即系统的状态和观测方程，如下：

$$x_k = ax_{k-1} + bu_k$$

$$z_k = cx_k + v_k$$

引入新的成分将现实更好地进行建模之后，对于求解过程中用到的5条预测和更新方程进行修改如下（第1条方程因为 $bu_k$被改动；第3、4、5条方程在奇奇怪怪的地方加了 $c$（orz这块没去深究））。

预测方程：

$$\hat{x_k} = a\hat{x_{k-1}} + bu_k$$

$$p_k = ap_{k-1}a$$

更新方程：

$$g_k = p_kc/(cp_kc + r)$$

$$\hat{x_k} \leftarrow \hat{x_k} + g_k(z_k -c\hat{x_k})$$

$$p_k \leftarrow (1-g_kc)p_k$$

假设飞行员平稳地上拉摇杆，此时如果将 $u_k$设置为随着时间线性增大的一个值（相当于平稳拉动摇杆时发动机能提供一个稳定的向上的力，即稳定的向上的加速度，从而飞机向上的分速度稳定线性增大，则单位时间内高度的改变量也呈线性递增），将其设置为2 * i（i为时间），以下MATLAB代码演示了对于方程进行修正后的卡尔曼滤波效果。

% 该程序用于演示加入了控制信号后的卡尔曼滤波效果

% Author CSDN @Charin_Wing

a = 0.9;% 定义a
r = 100;% 定义检测器精度
c = 1;% 定义传感器数据与待测数据换算系数
b = 0.5;% 定义控制信号换算系数
n = 50;% 时间（迭代次数）

for i = 1:n
    u(i) = 2 * i;% 将控制信号设置为随着时间线性增大的一个值
end

x(1) = 1000;% 定义x(1)
% 定义预测值x_k（由a、b和控制信号得出）
for i = 2:n
    x(i) = x(i-1) * a + b * u(i);
end

z = randi([-r, r], 1, n) + x;% 观测值为随机噪声加上预测值

p = ones(1, n);% 初始值p_0设置为1（防止p_0为0后面计算出来的p_k都为0）
t = (1:n);% 横坐标时间数组（可以理解为 1~n s内）
g = ones(1, n);%定义g_k

x_hat = zeros(1, n);% 初始化预测值存放的数组
x_hat(1) = z(1);% 第一个预测值直接设置为观测值（g1设置为1）

for i  = 2:n
    p(i) = a^2 * p(i-1);
    g(i) = p(i)*c/(p(i)*c^2+r);% 求出此时的g
    x_hat(i) = x_hat(i-1) * a + b * u(i) + g(i) * (z(i) - c*(x_hat(i-1) * a + b * u(i)));% 利用前一个值和g、z求出当前估计值
end

%以下是绘图函数
color1 = [0.453, 0.516, 0.711];
plot(t, x, 'Color',color1, 'LineWidth',1.5);
%legend(’Interpolation Polynomial’, ’Data Points’);
xlabel('t');
ylabel('x_hat');
title('Test for Kalman Filter');
hold on;

color2 = [0.59, 0.844, 0.711];%106, 177, 199
plot(t, z, 'Color',color2, 'LineWidth',1.5);

color3 = [0.863, 0.394, 0.445];%151, 216, 182
plot(t, x_hat, 'Color',color3, 'LineWidth',1.5);

legend('先验值x_k', '带随机噪声的检测器数据z_k', '经过Kalman滤波器处理后的估计值x_{hat}');

输出

线性代数

给系统添加速度

上述建模直接利用了当前高度与上一次高度之间的关系，而实际情况中，根据物理模型列出的方程中，往往包含速度、加速度等量，为了得到更贴合物理现实，从而与实际应用具有更丝滑的接口，我们需要加入更多的方程来体现中间量之间的关系。

引入数学家创造的线性代数，可以将这些方程进行形式上的归一化。修正后的先验方程形式如下：

$$\begin{bmatrix}distance_k\\velocity_k\\acceleration_k\end{bmatrix} = \begin{bmatrix}1×tep&0\\0&1×tep\\0&0&1\end{bmatrix}\begin{bmatrix}distance_{k-1}\\velocity_{k-1}\\acceleration_{k-1}\end{bmatrix}$$

在这个矩阵方程中，我们尝试建立一个离散的运动模型，其中的timestep是离散系统的步长。

第一行是利用上一时刻的距离和速度对下一时刻的距离进行了预测，此处尝试让距离只依赖于前一时刻的距离和速度，而没有利用加速度数据（对于极小的timestep，经过平方后乘上acceleration趋近于0）。

同样的，使速度只依赖于前一时刻的速度、和加速度；使加速度和前一时刻相等。

于是修改之后的系统状态方程如下

$$x_k = Ax_{k-1} + Bu_k$$

x、u：向量

A、B：矩阵

同理，得出加入噪声的传感器数据方程：

$$z_k = Cx_k + v_k$$

以上两条方程即与原先建立的两条模型方程对应，其中所有的量都是矩阵形式。而一开始没有加入速度等变量的方程，可以看作是现在得到的这个线性代数方程的特殊情况，相当于每个矩阵中只有一个位置具有非零值。

在线性代数表示的变量间关系的框架下重写卡尔曼滤波的预测更新方程。

预测方程

$$\hat{x_k} = A\hat{x_{k-1}} + Bu_k$$

$$P_k = AP_{k-1}A^T$$

将原先自变量的部分改成矩阵形式。此外，对于除法修改成取逆；另外要注意有关$p_k$的方程中之所以写成$ap_{k-1}a$，是与矩阵乘法运算不可逆有关，而在矩阵运算中，后一个$a$实际上要写成转置形式（在右边的系数都写成转置形式）。

更新方程

$$G_k = P_kC^T (CP_kC^T + R)^{-1}$$

$$\hat{x_k} \leftarrow \hat{x_k} + G_k(z_k - C\hat{x_k})$$

$$P_K \leftarrow (I - G_kC)P_k$$

传感器融合

借助线性代数可以使卡尔曼滤波器拥有一个非常有价值的能力——传感器融合。

考虑一个飞机系统的多个传感器，每个传感器对应状态的一个已知部分：一个显示海拔高度的气压计barometer，一个显示航向的罗盘compass，一个显示空速的流速计pitot tube。假设这些传感器精确无噪声，观测方程：

$$\begin{bmatrix}barometer_k\\compass_k\\pitot_k\end{bmatrix} = \begin{bmatrix}1&0&0\\0&1&0\\0&0&1\end{bmatrix}\begin{bmatrix}altitude_{k-1}\\heading_{k-1}\\airspeed_{k-1}\end{bmatrix}$$

即 $z_k = Cx_{k-1} + v_k$形式

（上面为了简化表述将C设置为单位阵）

接下来考虑有多个高度传感器的情况，比如除了barometer气压计，我们还有一个GPS，也可以用于显示高度，于是后验方程变为了（最左侧传感器数据矩阵中加入了gps信息）

$$\begin{bmatrix}barometer_k\\compass_k\\pitot_k\\gps_k\end{bmatrix} = \begin{bmatrix}1&0&0\\0&1&0\\0&0&1\\1&0&0\end{bmatrix}\begin{bmatrix}altitude_{k-1}\\heading_{k-1}\\airspeed_{k-1}\end{bmatrix}$$

Sensor 和 状态量之间不再是one-to-one correspondence的关系。

任何一个这样的系统都能为我们提供传感器融合的机会，也就是说，将多个传感器的观测数据结合起来，从而推断出系统一部分状态数据。

传感器融合的示例

矩阵形式下预测和更新公式中的R矩阵：

在单传感器的示例中，我们将 $r$定义为传感器在其均值周围的变化量，而对于有两个以上传感器的例子，R是一个包含每对传感器之间的协方差的矩阵，而该矩阵对角线上的元素即为每个传感器的 $$r$$值。对角线外的元素即为协方差，表示一个传感器噪声随另一个传感器噪声变化的程度。在本示例中，以及许多实际应用的情况中，协方差被设置为0。

接下来借助一个示例对于式子中的几个矩阵进行理解。

假设对于两个温度传感器测得数据进行了融合。

假设在温度稳定的气候条件下观察温度计。发现它们的测量值平均波动为0.8℃，则其读数的标准差=0.8，即方差=0.8 * 0.8=0.64，由此得到R矩阵：

$$R = \begin{bmatrix}0.64&0\\0&0.64\end{bmatrix}$$

对于 $P_k$矩阵， $P_k$为第 $k$步估计过程的协方差。

$P_k$表征了估计过程的置信度，而 $R$表征了传感器数据的置信度

在以上设定的情境下，因为只有温度这一个需要估计的状态量， $P_k$的大小为1 x 1（仅仅表示这一个需要估计的状态量与它自身的协方差）。

对于 $G_k$而言，他是一个在每一个迭代步骤中，由 $R$和 $P_k$的值得出的矩阵（ $G_k$中的每一个数据都是由 $R$和 $P_k$中的协方差数据得出的），而 $G_k$的大小为1 x 2，因为它将单个需要预测的状态量与两个传感器观测值联系在一起。

传感器融合建模

假设一个先验模型中 $\hat{x_k} = \hat{x_{k-1}}$的模型，即温度值本被设定为不变的模型。

但是由于过程噪声process noise的存在，实际情况中温度存在一定的抖动 （实际上如果不在这一步建模中加入这个过程噪声，这个模型最终呈现的结果中卡尔曼滤波的效果将是很差的。而事实证明尽管很小的过程噪声值，都能使卡尔曼滤波表现出它的滤波性能）

在上面的讨论中，过程噪声 $w_k$在先验模型中的存在形式如下：

$$x_k = ax_{k-1} + w_k$$

然而在多传感器模型中，与我们将传感器噪声使用协方差矩阵$R$表示一样，过程噪声同样使用矩阵形式表示。【而过程噪声矩阵 $Q$的维数实际上与 第 $k$步估计过程的协方差$P_k$保持一致，比如单个温度状态量下$P_k$为1 x 1表示温度状态量与它自身的协方差，则此时 $Q$实际上也为1 x 1】

于是，上述讨论下，原本的预测-更新方程变更为如下形式。

预测方程：

$$\hat{x_k} = A\hat{x_{k-1}} + Q$$

$$P_k = AP_{k-1}A^T + Q$$

更新方程

$$G_k = P_kC^T(CP_kC^T + R)^{-1}$$

$$\hat{x_k} \leftarrow \hat{x_k} + G_k(z_k - C\hat{x_k})$$

$$P_k \leftarrow (I - G_kC)P_k$$

根据以上方程在本文下方参考的网址中有示例代码，可以演示不同参数的时候卡尔曼滤波的效果，调参可以发现在Q值=0的时候滤波器出现失真。同时，参考中引用的网址中提供了展示多传感器融合的github demo的网址https://github.com/simondlevy/SensorFusion

非线性

线性代数使我们能够将复杂的算法写成一个紧凑的形式，然而线性代数仅仅能够表示线性关系。

自然界的大部分过程是非线性的，我们所看到的直线大多数是人造的（建筑的外形、道路）；自然界的树叶、花鸟虫都有着曲线的外形。而对于传感器，它虽然是人造的，但是有着物理原理，因此它们的特性也是非线性的。

一个非线性的Kalman Filter

将非线性关系分段化，而在每一段中使用线性近似（微分和求导的思想，斜率为分段点处的一阶导数），将线性近似的每一段拼合起来就得到及其接近原来的线性关系的图线，如图：

注意以上的非线性指的是传感器测得的直接数据，与所要预测的那个状态量之间的转换，为非线性的。即传感器物理原理与目标量之间为非线性关系。

在以下示例中，给出一个模型，为了简化模型，将重点突出在非线性的处理上，该模型设置为没有控制信号、有过程噪声、单传感器、单状态量。

首先给出使用原来的线性卡尔曼滤波器列出的方程。

模型：

$$x_k = x_{k-1} + w_k$$

$$z_k = cx_k + v_k$$

预测：

$$\hat{x_k} = \hat{x_{k-1}}$$

$$p_k = p_{k-1} + q$$

更新：

$$g_k = p_kc(cp_kc + r)^{-1} $$

$$\hat{x_k} \leftarrow \hat{x_k} + g_k(z_k - c\hat{x_k})$$

$$p_k \leftarrow (1 - g_kc)p_k$$

设初始非线性方程为 $h$；将 $c_k$设为在timestep为 $k$时的一阶导数。

以下分别给出非线性卡尔曼滤波的模型方程、预测方程、更新方程。

模型：

$$x_k = x_{k-1} + w_k$$

$$z_k = h(x_k) + v_k$$

预测：

$$\hat{x_k} = \hat{x_{k-1}}$$

$$p_k = p_{k-1} + q$$

更新：

$$g_k = p_kc_k(c_kp_kc_k + r)^{-1} $$

$$\hat{x_k} \leftarrow \hat{x_k} + g_k(z_k - h(\hat{x_k}))$$

$$p_k \leftarrow (1 - g_kc_k)p_k$$

使用在timestep = k时的斜率 $c_k$来代替原有线性关系中的 $a$

对于线性卡尔曼滤波器，其中的常量c是根据非线性方程近似取的一个 $c$值，在整个检测过程中使用这个近似的 $c$值构造的线性关系来代替原来的非线性关系。

以上这两种卡尔曼滤波器（线性和非线性模型）在参考的文章中给出了一个比较模型，截图如下：

尝试修改参数之后可以发现对近似取的 $c$的值的大小实际上不好把控而且调整后不管怎样效果都没有非线性的Kalman Filter好。

非线性 & 多状态量系统（雅可比矩阵）

以一个2*状态量，3*传感器的系统为例，我们可以将观测方程写为

$$z_k = \begin{bmatrix}c_{11}&c_{12}\\c_{21}&c_{22}\\c_{31}&c_{32}\end{bmatrix}\begin{bmatrix}x_{k1}\\x_{k2}\end{bmatrix} = \begin{bmatrix}c_{11}x_{k1}+c_{12}x_{k2}\\c_{21}x_{k1}+c_{22}x_{k2}\\c_{31}x_{k1}+c_{32}x_{k2}\end{bmatrix}$$

每个传感器的值是两个待测状态量的线性组合，线性组合的系数记在第一个矩阵中。

在非线性系统中，这些系数实际上是传感器对于每个状态量的函数的一阶偏导。这些一阶偏导组合的矩阵，称为雅可比矩阵。

【百度百科：在向量微积分中，雅可比矩阵是一阶偏导数以一定方式排列成的矩阵，其行列式称为雅可比行列式】

利用雅可比矩阵表示的模型

模型方程：

$$x_k = f(x_{k-1}, u_k) + w_k$$

$$z_k = h(x_k) + v_k$$

预测方程：

$$\hat{x_k} = f(\hat{x_{k-1}}, u_k)$$

$$P_k = F_{k-1}P_{k-1}F_{k-1}^T + Q_{k-1}$$

更新方程：

$$G_k = P_kH_k^T(H_kP_kH_k^T + R)^{-1} $$

$$\hat{x_k} \leftarrow \hat{x_k} + G_k(z_k - h(\hat{x_k}))$$

$$P_k \leftarrow (I - G_kH_k)P_k$$

其中， $f$表示状态转移方程的雅可比矩阵，参数为 $x_{k-1}$和 $u_k$，即 $f$实际上包含了 $x_k$对于 $x_{k-1}$和 $u_k$的偏导数。

而 $H_k$是传感器函数 $h$的雅可比矩阵。

参考

https://blog.csdn.net/xiaolong361/article/details/90116202

https://simondlevy.github.io/ekf-tutorial/