卡尔曼滤波原理(二)：扩展卡尔曼

1、理论部分

上一篇介绍了线性卡尔曼滤波器，当系统为线性高斯模型时，滤波器能给出最优的估计，但是实际系统总是存在不同程度的非线性，如平方、三角关系、开方等。对于非线性系统，可以采用的一种方法是通过线性化方法将非线性系统转换为近似的线性系统，即为EKF，核心思想是：围绕滤波值将非线性函数展开成泰勒级数并略去二阶及以上项，得到一个近似的线性化模型，然后应用卡尔曼滤波完成状态估计。

系统方程

各个变量意义同上一节，其中f和h代表状态和观测的非线性函数。

在扩展卡尔曼滤波中，状态的预测以及观测值的预测由非线性函数计算得出，线性卡尔曼滤波中的状态转移矩阵A阵和观测矩阵H阵由f和h函数的雅克比矩阵代替，假设状态有n维，则求法如下：

有了上面矩阵的计算方法，EKF滤波过程同线性卡尔曼滤波相同，公式如下

2、实践部分

根据图中情景，选取横向位置、速度，纵向位置、速度为状态量，列出下面非线性状态方程及观测方程

根据状态方程和观测方程，计算雅克比矩阵如下

close all;
clear all;
%% 真实轨迹模拟
kx = .01; ky = .05; % 阻尼系数
g = 9.8; % 重力
t = 15; % 仿真时间
Ts = 0.1; % 采样周期
len = fix(t/Ts); % 仿真步数
dax = 3; day = 3; % 系统噪声
X = zeros(len,4);
X(1,:) = [0, 50, 500, 0]; % 状态模拟的初值
for k=2:len
x = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); vy = X(k-1,4);
x = x + vx*Ts;
vx = vx + (-kx*vx^2+dax*randn(1,1))*Ts;
y = y + vy*Ts;
vy = vy + (ky*vy^2-g+day*randn(1))*Ts;
X(k,:) = [x, vx, y, vy];
end
%% 构造量测量
dr = 8; dafa = 0.1; % 量测噪声
for k=1:len
r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1);
a = atan(X(k,1)/X(k,3))*57.3 + dafa*randn(1,1);
Z(k,:) = [r, a];
end
%% ekf 滤波
Qk = diag([0; dax/10; 0; day/10])^2;
Rk = diag([dr; dafa])^2;
Pk = 10*eye(4);
Pkk_1 = 10*eye(4);
x_hat = [0,40,400,0]';
X_est = zeros(len,4);
x_forecast = zeros(4,1);
z = zeros(4,1);
for k=1:len
% 1 状态预测
x1 = x_hat(1) + x_hat(2)*Ts;
vx1 = x_hat(2) + (-kx*x_hat(2)^2)*Ts;
y1 = x_hat(3) + x_hat(4)*Ts;
vy1 = x_hat(4) + (ky*x_hat(4)^2-g)*Ts;
x_forecast = [x1; vx1; y1; vy1]; %预测值
% 2 观测预测
r = sqrt(x1*x1+y1*y1);
alpha = atan(x1/y1)*57.3;
y_yuce = [r,alpha]';
% 状态矩阵
vx = x_forecast(2); vy = x_forecast(4);
F = zeros(4,4);
F(1,1) = 1; F(1,2) = Ts;
F(2,2) = 1-2*kx*vx*Ts;
F(3,3) = 1; F(3,4) = Ts;
F(4,4) = 1+2*ky*vy*Ts;
Pkk_1 = F*Pk*F'+Qk;
% 观测矩阵
x = x_forecast(1); y = x_forecast(3);
H = zeros(2,4);
r = sqrt(x^2+y^2); xy2 = 1+(x/y)^2;
H(1,1) = x/r; H(1,3) = y/r;
H(2,1) = (1/y)/xy2; H(2,3) = (-x/y^2)/xy2;
Kk = Pkk_1*H'*(H*Pkk_1*H'+Rk)^-1; %计算增益
x_hat = x_forecast+Kk*(Z(k,:)'-y_yuce); %校正
Pk = (eye(4)-Kk*H)*Pkk_1;
X_est(k,:) = x_hat;
end
%%
figure, hold on, grid on;
plot(X(:,1),X(:,3),'-b');
plot(Z(:,1).*sin(Z(:,2)*pi/180), Z(:,1).*cos(Z(:,2)*pi/180));
plot(X_est(:,1),X_est(:,3), 'r');
xlabel('X');
ylabel('Y');
title('EKF simulation');
legend('real', 'measurement', 'ekf estimated');
axis([-5,230,290,530]);

仿真结果

https://blog.csdn.net/u012936940/article/details/77249245

卡尔曼滤波原理(二)：扩展卡尔曼相关推荐

卡尔曼滤波原理二：扩展卡尔曼
1.理论部分上一篇介绍了线性卡尔曼滤波器,当系统为线性高斯模型时,滤波器能给出最优的估计,但是实际系统总是存在不同程度的非线性,如平方.三角关系.开方等.对于非线性系统,可以采用的一种方法是通过线性 ...
卡尔曼滤波原理详解（二）
卡尔曼滤波原理详解(二) 前言卡尔曼增益推导总结前言本文是对卡尔曼滤波学习的记录,主要参照了DR_CAN老师的视频进行学习.视频专栏链接:DR_CAN卡尔曼滤波视频专栏.虽然网上有很多卡尔曼滤 ...
【Matlab】扩展卡尔曼滤波器原理及仿真（初学者入门专用）
文章目录 0.引言及友情链接 1.场景预设 2.扩展卡尔曼滤波器 3.仿真及效果 0.引言及友情链接 \qquad卡尔曼滤波器(Kalman Filter, KF)是传感器融合(Sensor Fusi ...
说不尽的卡尔曼 | 详解扩展卡尔曼滤波器
作者 | 火山编辑 | 空中机器人前沿点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取点击进入→自动驾驶之心[目标跟踪]技术交流群后台回复[卡尔曼滤波] ...
自动驾驶算法-滤波器系列（一）——详解卡尔曼滤波原理
详解卡尔曼滤波原理什么是卡尔曼滤波? 我们能用卡尔曼滤波做什么? 卡尔曼滤波是如何看到你的问题的使用矩阵来描述问题外部控制量外部干扰用测量量来修正估计值融合高斯分布将所有公式整合起来总 ...
卡尔曼滤波器、扩展卡尔曼滤波器、无向卡尔曼滤波器的详细推导
这段时间做轴承故障诊断和预测的时候,需要一个针对已经获取了特征向量的工具来对轴承故障状态进行估计和预测.卡尔曼滤波器可以实现对过去.当前和未来目标位置的估计,所以想通过卡尔曼滤波器的设计思路找到一些灵 ...
卡尔曼滤波原理及实现
卡尔曼滤波原理及实现前一段时间,做项目研究了一下卡尔曼滤波,并且在项目当中实现了一个物体跟踪的功能,所以,借着新鲜劲儿,本次博客对卡尔曼滤波进行一次整理. 卡尔曼滤波是什么卡尔曼滤波能做什么卡尔 ...
卡尔曼滤波原理详解（一）
卡尔曼滤波原理详解(一) 前言数据融合的思想例子引入卡尔曼增益推导前言本文是对卡尔曼滤波学习的记录,主要参照了DR_CAN老师的视频进行学习.视频专栏链接:DR_CAN卡尔曼滤波视频专栏.虽 ...
卡尔曼滤波原理及c++实现
卡尔曼滤波原理及c++实现滤波原理卡尔曼滤波是一种最优估计算法. 用处:1)利用可测量值估算无法测量的量.2)对有测量噪声的物理量进行估计.常用于制导和导航.计算机视觉等. 状态估计器卡尔曼滤波 ...

卡尔曼滤波原理(二)：扩展卡尔曼

1、理论部分

2、实践部分

卡尔曼滤波原理(二)：扩展卡尔曼相关推荐

最新文章

热门文章