扩展卡尔曼滤波EKF

嗣音

分类：学习笔记

个人专栏：概率机器人

发布时间 2023.07.20阅读数 1296 评论数 0

记录一下对扩展卡尔曼滤波(Extended Kalman Filter, EKF)的理解。

参考《概率机器人》、《卡尔曼滤波原理及应用-MATLAB仿真》
此篇之前所衔接的博客是《卡尔曼滤波》：https://www.guyuehome.com/43705

一、背景、原理

1.1 线性到非线性扩展

观测结果是状态的线性函数，并且下一状态是以前状态的线性函数，这两个假设对KF算法的正确性非常重要。
但是，实际系统总是存在不同程度的非线性，典型的非线性函数关系包括平方关系、对数关系、指数关系、三角函数关系等。
对于非线性系统滤波问题，常用的处理方法是利用线性化技巧将其转化为一个近似的线性滤波问题。故，我们希望把卡尔曼滤波的结果扩展到非线性系统中，称为扩展卡尔曼滤波方法（Extended Kalman Filter，EKF）。

EKF滤波建立在KF滤波的基础上，其核心思想是：对一般的非线性系统，在某个点附近考虑运动方程及其观测方程的一阶泰勒展开，只保留一阶项，得到一个近似的线性化模型，然后用KF滤波完成对目标的滤波估计等处理。

对于非线性的理解，这一点非常重要！！！
非线性意味着，在观测方程里，观测量或许可以跟状态量不在一个维度。
例如，对于状态变量 X(t) = [x(t), y(t)]，观测方程 Z(t) = sqrt((x(t) - 100)^2 + t(y(t) - 100)^2) + V 。这里的观测实际是算了当前位置与观测位置的欧式距离，这里就由于C的非线性，导致 X(t) 是二维的，但 Z(t) 却是一维的。

1.2 算法内容

初始化状态、观测、协方差。
一阶线性化状态方程，得到状态矩阵 A 的雅克比矩阵 G。也就是状态方程对于μ(t-1)的一阶导函数对应的结果
一阶线性化观测方程，得到状态矩阵 C 的雅克比矩阵 H。也就是观测方程对于_μ(t)的一阶导函数对应的结果_
g(u(t), μ(t-1))就是状态方程，由于非线性故用函数表达。
h(_μ(t))就是观测方程，由于非线性故用函数表达。_
计算预测置信度_bel(x(t))的均值_μ(t)和方差为_Σ(t)。对应第2、第3行。_
求卡尔曼增益 K ，对应第4行。
通过综合测量z(t)得到期望的置信度bel(x(t))的均值μ(t)和方差为Σ(t)。对应第6、第7行。
循环迭代传递。

其推导这里不做记录，可以参照《概率机器人》
其具体步骤大致与KF一致，参考KF：https://www.guyuehome.com/43705

EKF与KF不同之处主要有两点：

第2行和第5行公式格式不同，KF的线性预测在EKF中由其非线性扩展替代。
第2行关于状态预测：EKF里是 g(u(t), μ(t-1))；KF里是 A.μ(t-1) + B.μ(t)
第5行关于测量更新：EKF里是 h(_μ(t))；KF里是 C._μ(t)
用雅克比矩阵 G 和 H 取代了KF 中对应的线性系统矩阵 A、B和C。
雅克比矩阵就是多元函数的偏导数组成的矩阵：https://baike.baidu.com/item/%E9%9B%85%E5%8F%AF%E6%AF%94%E7%9F%A9%E9%98%B5/10753754?fr=ge_ala
雅克比矩阵 G 对应矩阵 A 对状态向量的雅克比矩阵。
雅克比矩阵 H 对应矩阵 C 对状态向量的雅克比矩阵。

二、实例一：简单非线性系统EKF设计

2.1 问题描述

可以看到状态方程是非线性的，W(k)为过程噪声，其为均值为0、方差为R = 1的高斯白噪声。
观测方差也是非线性的，V(k)为测量噪声，其为均值为0、方差为Q = 10的高斯白噪声。
该问题的主要意义是，它是一个一维状态，状态方程和观测方程都是非线性的。

2.2 手动解算

略，看matlab代码及注释。

2.3 matlab实现

%% 根据系统描述，初始化一些值
clc;clear;
N=50;                       % 采样次数

X = zeros(1,N);             % 状态值 真值
Z = zeros(1,N);             % 观测值 z(t)
Xekf = zeros(1,N);          % kf算法里的状态均值 μ(t)
Pekf = zeros(1,N);          % kf算法里的状态方差 Σ(t)

R = 1;                      % 过程噪声方差
Q = 10;                     % 测量噪声方差

% 初值给定
X(1) = 0.1;
Z(1) = X(1)^2/20;
Xekf(1) = Z(1);             % 通过第一个观测值来更新初始状态均值 μ(1)
Pekf(1) = eye(1);           % 初始方差（估摸着）


%% 根据噪声，模拟实际数据 和 测量数据
W = sqrt(R)*randn(1,N);
V = sqrt(Q)*randn(1,N);
for t = 2:N
    X(t) = 0.5*X(t-1) + 2.5*X(t-1)/(1 + X(t-1)^2) + 8*cos(1.2*t) + W(t);
    Z(t) = X(t)^2/20 + V(t);
end


% 函数 f(X) = 0.5*X + 2.5*X/(1 + X^2) 对 X 的偏导数为：
% ∂f/∂X = f'(X) = 0.5 + (2.5 - 2.5*X^2)/ (1 + X^2)^2

% 函数 f(X) = X^2/20 对 X 的偏导数为：
% ∂f/∂X = f'(X) = x / 10


%% 卡尔曼滤波
for t=2:N
    X_pre = 0.5*Xekf(t-1) + 2.5*Xekf(t-1)/(1 + Xekf(t-1)^2) + 8*cos(1.2*t);   % 预测状态均值 _μ(t)，对应g(u(t), _μ(t-1))

    G = 0.5 + (2.5 - 2.5*X_pre^2)/(1 + X_pre^2)^2;        % 一阶线性化状态方程，得到状态矩阵 A 的雅克比矩阵 G
    P_pre = G*Pekf(t-1)*G' + R;           % 预测状态方差 _Σ(t)

    H = X_pre/10;                         % 一阶线性化观测方程，得到状态矩阵 C 的雅克比矩阵 H。
    K = P_pre*H'/(H*P_pre*H' + Q);        % 更新卡尔曼增益 K

    Z_pre = X_pre^2/20;                   % 预测观测值，用于计算真实测量向量z(t)的偏差，对应 h(_μ(t))
    Xekf(t) = X_pre + K*(Z(t) - Z_pre);   % 更新状态均值 μ(t)
    Pekf(t) = (eye(1) - K*H)*P_pre;       % 更新状态方差 Σ(t)
end


%% 分析误差项
Err_Messure=zeros(1,N);
Err_Kalman=zeros(1,N);
for t=1:N
    Err_Messure(t)=abs(Z(t)-X(t));
    Err_Kalman(t)=abs(Xekf(t)-X(t));
end


%% 画图
t=1:N;
figure('Name','Kalman Filter Simulation','NumberTitle','off');
plot(t,X,'-r.',t,Z,'-k.',t,Xekf,'-g.');
legend('real','measure','kalman extimate');         
xlabel('sample time');
ylabel('temperature');
title('Kalman Filter Simulation');

figure('Name','Error Analysis','NumberTitle','off');
plot(t,Err_Messure,'-b.',t,Err_Kalman,'-k.');
legend('messure error','kalman error');         
xlabel('sample time');
ylabel('error');
title('Error Analysis');

figure('Name','Kalman Filter Variance','NumberTitle','off');
plot(t,Pekf,'-b');       
xlabel('sample time');
ylabel('Variance');
title('Kalman Filter Variance');

2.4 结果分析

真实值、测量值、EKF得到的值如下，可以看到明显的效果。

测量值、EKF得到的值与真实值做差对比如下。

这个一维状态的分布，其方差也是一维了，我们可以观测其方差变化。

三、实例一：目标跟踪问题

2.1 问题描述

这里，我们把随机扰动U(k)看作噪声，也就是系统方程为：X(t) = AX(t-1) + W(t)，过程噪声W(t)均值为0、方差表示为R
其中：

而对于过程噪声W(t)的方差R，要满足一定的规律，也就是与U(k)对应。
这里设置：

这里观测噪声V(t)均值为0、方差表示为Q = 5。
可以看到，由于非线性变换，X(t) 是二维的，但 Z(t) 却是一维的。

这个系统的特点是，状态是多维的分布，状态方程为线性而观测方程为非线性。

2.2 手动解算

略，看matlab代码及注释。

2.3 matlab实现

%% 根据系统描述，初始化一些值
clc;clear;
T = 1;                        % ∆t
N = 60;                       % 采样次数

X = zeros(4,N);                 % 状态值 真值
Z = zeros(1,N);                 % 观测值 z(t)
Xstation = [200,0,300,0];           % 观测基站的位置
Xekf = zeros(4,N);              % kf算法里的状态均值 μ(t)
Pekf = zeros(4,4,N);            % kf算法里的状态方差 Σ(t)


A = [1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];  % 状态转移方程相关


R = 1e-3*diag([0.5,1,0.5,1]) ;  % 过程噪声的方差   
Q = 5;                          % 测量噪声方差

% 初值给定
X(:,1) = [-100,2,200,20];
Z(1) = Dist(X(:,1),Xstation);
Xekf(:,1) = X(:,1);             % 更新初始状态均值 μ(1)，这里假设我们知道初始的实际位置
Pekf(:,:,1) = eye(4);           % 初始方差（估摸着）


%% 根据噪声，模拟实际数据 和 测量数据
for t = 2:N
    X(:,t) = A*X(:,t-1) + sqrtm(R)*randn(4,1);  % 注意这里噪声的协方差与B矩阵相关
    Z(t) = Dist(X(:,t),Xstation) + sqrtm(Q)*randn(1);
end


% 函数 f(X, Y) = sqrt( (X - a)^2 + (Y - b)^2 ) 对 X 和 Y 的偏导数分别为：
% ∂f/∂X = (X - a) / ( sqrt( (X - a)^2 + (Y - b)^2 ) )
% ∂f/∂Y = (Y - b) / ( sqrt( (X - a)^2 + (Y - b)^2 ) )



%% 卡尔曼滤波
for t = 2:N
    X_pre = A*Xekf(:,t-1);                  % 预测状态均值 _μ(t)，注意这里状态转移方程是线性的
    P_pre = A*Pekf(:,:,t-1)*A' + R;    % 预测状态方差 _Σ(t)

    H = [(X_pre(1,1)-Xstation(1))/Dist(X_pre, Xstation), 0, ...
        (X_pre(3,1)-Xstation(3))/Dist(X_pre, Xstation), 0]; % 一阶线性化观测方程，得到状态矩阵 C 的雅克比矩阵 H。

    K = P_pre*H'/(H*P_pre*H' + Q);       % 更新卡尔曼增益 K

    Xekf(:,t) = X_pre + K*(Z(:,t) - Dist(X_pre, Xstation));    % 更新状态均值 μ(t)
    Pekf(:,:,t) = (eye(4) - K*H)*P_pre;     % 更新状态方差 Σ(t)
end


%% 分析误差项
Err_Messure = zeros(1,N);
Err_Kalman = zeros(1,N);
for t=1:N
    Err_Messure(t) = abs(Z(t)-Dist(X(:,t),Xstation));
    Err_Kalman(t) = Dist(X(:,t),Xekf(:,t));
end


%% 画图
t=1:N;
figure('Name','Kalman Filter Simulation','NumberTitle','off');
plot(X(1,:),X(3,:),'-k.',Xekf(1,:),Xekf(3,:),'-r.');
legend('real','kalman extimate');         
xlabel('x');
ylabel('y');
title('Kalman Filter Simulation');

% figure('Name','Kalman Filter Simulation Vx','NumberTitle','off');
% plot(t,X(2,:),'-k.',t,Xekf(2,:),'-r.');
% legend('real','kalman extimate');         
% xlabel('sample time');
% ylabel('Vx');
% title('Kalman Filter Simulation Vx');
% 
% figure('Name','Kalman Filter Simulation Vy','NumberTitle','off');
% plot(t,X(4,:),'-k.',t,Xekf(4,:),'-r.');
% legend('real','kalman extimate');         
% xlabel('sample time');
% ylabel('Vx');
% title('Kalman Filter Simulation Vy');

figure('Name','Error Analysis','NumberTitle','off');
plot(t,Err_Messure,'-b.',t,Err_Kalman,'-k.');
legend('messure error','kalman error');         
xlabel('sample time');
ylabel('error');
title('Error Analysis');


function d=Dist(X1,X2)
    d=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 );
end

2.4 结果分析

可以看到这种观测方式，是极其容易发散的。。。

over~

在KF理解的基础上去理解EKF的使用，会更加轻松一些。
附玄月大人~

EKF

原创文章作者：嗣音。如若转载，请注明出处：古月居 https://www.guyuehome.com/43949

打赏 0

上一篇：卡尔曼滤波

下一篇：（一）最优化问题、一阶二阶梯度法

扩展卡尔曼滤波EKF

嗣音

一、背景、原理

1.1 线性到非线性扩展

1.2 算法内容

二、实例一：简单非线性系统EKF设计

2.1 问题描述

2.2 手动解算

2.3 matlab实现

2.4 结果分析

三、实例一：目标跟踪问题

2.1 问题描述

2.2 手动解算

2.3 matlab实现

2.4 结果分析

over~

为你推荐

【PyTorch】模型的保存和加载

C++之命令(Command)模式

Qt之QSS基础

gazebo入门教程（二）建立简单模型

树莓派 ROS 学习（二）小乌龟 turtlesim

工业机器人运动学与Matlab正逆解算法学习笔记（用心总结一文全会）（四）——雅可比矩阵

评论（0）

关于作者

嗣音

21

1

28

8

PX4的spi驱动，nuttx的spi硬件抽象

处女篇

精选eigen使用小记

相关推荐

精选DRM——学习篇0：概念认识

精选聚合物？锂离子？保护板？电子DIY中的锂电池相关知识大讲解

精选数字IC笔面基础，辨别有无入门的利器——STA相关知识点（静态时序分析）

GoWeb学习-第二天

CoDeSys开发经验总结

工作一年的CV算法工程师感想——技术篇

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

TA的专栏

数值最优化方法

概率机器人

视觉感知

其它

PX4

单片机

16届智能车竞赛创意组

专栏导航

爱你么么哒

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板