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:lenx = 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:lenr = 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]);

仿真结果

卡尔曼滤波原理二:扩展卡尔曼相关推荐

  1. 卡尔曼滤波原理(二):扩展卡尔曼

    1.理论部分 上一篇介绍了线性卡尔曼滤波器,当系统为线性高斯模型时,滤波器能给出最优的估计,但是实际系统总是存在不同程度的非线性,如平方.三角关系.开方等.对于非线性系统,可以采用的一种方法是通过线性 ...

  2. 卡尔曼滤波原理详解(二)

    卡尔曼滤波原理详解(二) 前言 卡尔曼增益推导 总结 前言 本文是对卡尔曼滤波学习的记录,主要参照了DR_CAN老师的视频进行学习.视频专栏链接:DR_CAN卡尔曼滤波视频专栏.虽然网上有很多卡尔曼滤 ...

  3. 【Matlab】扩展卡尔曼滤波器原理及仿真(初学者入门专用)

    文章目录 0.引言及友情链接 1.场景预设 2.扩展卡尔曼滤波器 3.仿真及效果 0.引言及友情链接 \qquad卡尔曼滤波器(Kalman Filter, KF)是传感器融合(Sensor Fusi ...

  4. 说不尽的卡尔曼 | 详解扩展卡尔曼滤波器

    作者 | 火山 编辑 | 空中机器人前沿 点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取 点击进入→自动驾驶之心[目标跟踪]技术交流群 后台回复[卡尔曼滤波] ...

  5. 自动驾驶算法-滤波器系列(一)——详解卡尔曼滤波原理

    详解卡尔曼滤波原理 什么是卡尔曼滤波? 我们能用卡尔曼滤波做什么? 卡尔曼滤波是如何看到你的问题的 使用矩阵来描述问题 外部控制量 外部干扰 用测量量来修正估计值 融合高斯分布 将所有公式整合起来 总 ...

  6. 卡尔曼滤波器、扩展卡尔曼滤波器、无向卡尔曼滤波器的详细推导

    这段时间做轴承故障诊断和预测的时候,需要一个针对已经获取了特征向量的工具来对轴承故障状态进行估计和预测.卡尔曼滤波器可以实现对过去.当前和未来目标位置的估计,所以想通过卡尔曼滤波器的设计思路找到一些灵 ...

  7. 卡尔曼滤波原理及实现

    卡尔曼滤波原理及实现 前一段时间,做项目研究了一下卡尔曼滤波,并且在项目当中实现了一个物体跟踪的功能,所以,借着新鲜劲儿,本次博客对卡尔曼滤波进行一次整理. 卡尔曼滤波是什么 卡尔曼滤波能做什么 卡尔 ...

  8. 卡尔曼滤波原理详解(一)

    卡尔曼滤波原理详解(一) 前言 数据融合的思想 例子引入 卡尔曼增益推导 前言 本文是对卡尔曼滤波学习的记录,主要参照了DR_CAN老师的视频进行学习.视频专栏链接:DR_CAN卡尔曼滤波视频专栏.虽 ...

  9. 卡尔曼滤波原理及c++实现

    卡尔曼滤波原理及c++实现 滤波原理 卡尔曼滤波是一种最优估计算法. 用处:1)利用可测量值估算无法测量的量.2)对有测量噪声的物理量进行估计.常用于制导和导航.计算机视觉等. 状态估计器 卡尔曼滤波 ...

最新文章

  1. 数据结构 - 最小堆最大堆
  2. 汇编语言(二十三)之求一个数的补数
  3. MySQL 之 存储过程
  4. SyntaxError: Identifier ‘XXX‘ has already been declared
  5. 被90%中国人误解了12年的斜杠
  6. 最详细虚拟机VMware安装CDLinux(12)
  7. 管理理论模型:PEST、5W2H、时间管理、生命周期、逻辑树、金字塔、SMART原则
  8. windows服务启动程序_在Windows上快速启动程序的5种方法
  9. ubuntu配置全局系统代理
  10. HTML+CSS+JS大作业:生态环境网站设计——环境保护主题-绿色环保 (9页) web期末作业设计网页_绿色环保大学生网页设计作业成品
  11. 从苹果创业神话看资本运营应集中优势兵力
  12. win11右键菜单缺少“新建“选项解决办法“
  13. Buffer的基本用法
  14. Python 最强 IDE 详细使用指南!
  15. 在Unity3d项目中利用Udp进行局域网内通信
  16. php发送指令给易语言,易语言发送信息代码数字指令编程整理
  17. 七星聚会!我在学堂在线获得的荣誉证书!(截至2017年8月12日)
  18. Hadoop2.7.5集群搭建
  19. POST与GET的区别深度比较分析
  20. C++ STL标准库:std::set std::multiset 插入元素insert() 擦除元素erase()的使用

热门文章

  1. 困扰?什么是面向对象什么是面向过程
  2. PLC高精度定时器(T法测速功能块)
  3. 计算机应用教学总结,计算机教学总结
  4. php如果登录失败弹出失败,登录失败或出现错误信息(无跳转、提示失败、出现错误信息)的解决方法...
  5. PHP 十六大魔术方法学习
  6. 网络:以太网类型(EthernetType)整理
  7. 大文件传输利器HFS的配置及简单使用
  8. (windows)如何删除删不掉的文件
  9. CST(CST Studio Suite)建立工程和基本绘图方法
  10. Java语音技术之FreeTTS