扩展卡尔曼滤波器（一）

Charin_Wing

已于 2024-04-21 23:50:38 修改

阅读量906

点赞数 20

文章标签：算法

于 2024-04-19 20:58:34 首次发布

本文链接：https://blog.csdn.net/2301_76677400/article/details/137976702

版权

Extended Kalman Filter EKF

对于Kalman Filter的理解

建模过程

Kalman filter经常用于处理在实际应用过程中的传感器噪声

比如海拔测量，

$altitude_{observe} = altitude_{real} + noise$

而我们希望达到的控制效果如下：

$altitude_{now} = a * altitude_{pre}$ （状态方程）(a设置为0.98)

现在就得到了两条式子

Kalman filter得到广泛运用的原因，就在于它让我们可以在已知观测值 $altitude_{observe}$ 、常数a和测量噪声总量noise的情况下，得到一个当前真实状态 $altitude_{real}$ 的比较好的估计值。

添加一个实际模型：飞机降落。而在飞机降落的过程中，除了传感器本身的噪声，飞机的抖动是不可避免的，运动方程可以修正如下

$altitude_{now} = a * altitude_{pre} + w$

w：过程噪声，过程中固有的，而不是观测带来的

但是考虑到集中精力研究其他部分，暂时忽略该噪声

并将上述两条方程简化为

$x_k = a * x_{k-1}$ 和 $x_k = z_k - v_k$ （运动方程以及观测量&噪声&实际值关系）

滤波基本过程&实例

Example

在变量上方加^（ $x \rightarrow \hat{x}$ ）：表示该变量是估计得到的。

Kalman filter：通过当前状态的观测值，以及上一状态的估计值，来估计当前状态（实际上即在获得观测值的前提下，还需要借助上一个值来优化观测值得到当前值），如下：

$\hat{x_k} = \hat{x_{k-1}} + \hat{g_k}(\hat{z_k} - \hat{x_{k-1}})$

$\hat{g_k}$ ：折衷的增益

考虑两个极端的情况，可以发现， $\hat{g_k}$ = 0时，当前值就等于上一个值；而 $\hat{g_k}$ = 1时，上一个值对当前值没有影响。因此 $\hat{g_k}$ 实际上可以被视为观测值对当前状态的影响因子（即 $\hat{g_k}$ 越大，观测值越可信）。【 $\hat{g_k}$ 是和观测值绑定的】

举例 $\hat{g_k} = 0.5; \hat{z_k} = 105; \hat{x_{k-1}} = 110$ ，则估计得到 $\hat{x_k}$ 的值为107.5。

Cal \^g_k

那么如何来求得直接表征了观测值可信度的 $\hat{g_k}$ 呢

增益的计算方法：通过噪声noise间接计算（每个观测值都和特定的噪声值相关）

我们可以知道噪声的平均水平r，反映传感器属性

$\hat{g_k} = p_{k-1}/(p_{k-1} + r)$

$\hat{p_k}$ ：递归计算得到的预测误差。r：传感器输出噪声的大致水平r。

所以可以理解成，这条式子中 $\hat{p_{k-1}}$ 就是这组数据均一稳定性的代表（指标），如果 $\hat{p_{k-1}}$ 足够大，则数据的均一性使得此时高度的检测值可以直接用上一时刻代替（当然实际情况中这种极端情况比较少见，一般都是 $\hat{p_{k-1}}$ 和 r 各自有一定的大小，使得数据处理过程不被另一方统治（即观测值和上一状态值处在协同纠正的一个良好状态下））；而相应地，如果 r 足够大，则这表明传感器的精度实际上处在一个根本测不准所要求数值的水平（一般也不会用这样的传感器orz），这个时候就直接把数据预测过程的统治权给到前一时刻的数值了。

考虑极端情况

预测误差 $\hat{p_{k}} = 0$

$\rightarrow$ 增益 $\hat{g_k }= 0$ 。即下一个状态估计将会和当前的状态估计相同。（预测误差 $p_{k-1} = 0$ -> 状态估计准确，不需要调整状态估计）

预测误差 $\hat{p_{k}} = 1$

增益 $\hat{g_{k}} = 1/(1+r)$ ，此时如果r = 0，则下一状态估计值会完全由观测值决定，但是随着 r 不断增大, 增益将会变的任意小，尽管前一个状态不应该沿用到当前状态，但是噪声实在是太大了（一般也不会买这样的传感器吧），我们将继续使用前一个状态估计作为当前的状态估计。

根据 $\hat{g_{k}}$ 和 $\hat{p_{k-1}}$ 可以迭代计算出 $\hat{p_{k}}$ ，即 $p_k = (1-\hat{g_k})p_{k-1}$

利用a得出的先验值

在以上例子中，我们仅仅利用了测量值和过去的数据对当前数据进行了预测，而实际上当前数据还有一个先验值（上面的计算得到的实际上属于后验值）：

$x_k = a x_{k-1}$

这个式子表示的是对于系统状态应该是什么的预测

此外，对于 $p_k$ 也存在一个利用 a 进行预测的先验值：

$p_k = ap_{k-1}a$

卡尔曼滤波：不断利用上面的先验、后验等式进行预测-更新；预测-更新...

利用上面得到的先验值和后验值运行Kalman Filter

先验值（预测值）（仅与 a 相关）

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

$p_k = ap_{k-1}a$

后验值（更新值）

$g_k = p_k/(p_k+r)$

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

$p_k \leftarrow (1-g_k)p_k$

最后两条式子：使用箭头表示该式是对当前值的更新（利用了其他参数修正原来的值再将修正后的结果再次赋给原先的变量）

测试过程同样建立在上述背景下，即飞机降落，运动学方程为飞机高度以 $\hat{x_k} = a\hat{x_{k-1}}$ 减小，这里a设置为0.75，得到一组预测解。

此外，高度传感器（检测器）精度设置为 $\pm200$ ，即r = 200，如下

k	1	2	3	4	5	6	7	8	9	10
$$x_k$$	1000	750	563	422	316	237	178	133	100	75
$$z_k$$
$$p_k$$	1	0.561
$$\hat{x_k}$$	1081
$$g_k$$		0.003

在对每一栏状态的预测中，前后状态变量关联如下：

由前一个 $p_k$ 值得到此时的 $p_k$ 值（先验公式）
由该 $p_k$ 值结合检测器的 r 计算出当前的 $g_k$ 值（比如该情境下设置 r 为200）
然后将前一步的预测值 $\hat{x_k}$ 和得到的 $g_k$ ，以及当前的 $z_k$ 代入式子求得当前步的预测值 $\hat{x_k}$

利用MATLAB编写代码验证滤波效果

初次接触卡尔曼滤波，为了加深理解，尝试使用MATLAB对滤波过程进行了验证，代码编写如下（为了减少经过随机处理模拟检测器数据后可能会输出检测器的值 < 0的情况，将 r 设置为100）：

% 该程序用于演示卡尔曼滤波效果

% Author CSDN @Charin_Wing

% 以下给出了预测和更新方程
% Predict
% xhat = a * xhat;
% p    = a * p * a;

% Update
% g    = p  / (p  + r);
% xhat = xhat + g * (z - xhat);
% p    = (1 - g) * p;
a = 0.75;% 定义a
r = 100;% 定义检测器精度
% 定义预测值x_k（由a得出）
x = [1000, 750, 563, 422, 316, 237, 178, 133, 100, 75];
z = randi([-r, r], 1, 10) + x;% 观测值为随机噪声加上预测值

p = ones(1, 10);% 初始值p_0设置为1（防止p_0为0后面计算出来的p_k都为0）

t = (1:10);% 横坐标时间数组（可以理解为1~10s内）

g = ones(1, 10);%定义g_k

x_hat = zeros(1, 10);% 初始化预测值存放的数组

x_hat(1) = z(1);% 第一个预测值直接设置为观测值（g1设置为1）

for i  = 2:10
    p(i) = a^2 * p(i-1);
    g(i) = p(i)/(p(i)+r);% 求出此时的g
    x_hat(i) = x_hat(i-1) * a + g(i) * (z(i) - x_hat(i-1) * a);% 利用前一个值和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}');

本代码中，首先定义了一个公式1得到的先验x_k值的数组x；然后对于该数组进行了 $\pm100$ 以内的随机数的叠加，从而模拟检测器测得的数据，记为数组z；接着根据上述讨论的卡尔曼滤波原理对数据进行了处理，估计得到数组x_hat。将最终得到的这三个数组对于t（假定为1~10s采集的10个数据）作图。