卡尔曼滤波器MATLAB实现（从一维到三维）
一、场景设置与效果图
假设一架无人机携带了一个气压计和GPS，定位精度都为1m，数据采样频率都为5Hz，100s时间螺旋上升40m，螺旋半径为20m。在无人机飞行过程中，1m的精度是不能很好满足要求的，现对采样得到的数据使用卡尔曼滤波，使得达到更高的精度。
最终效果如下：
二、卡尔曼滤波
卡尔曼滤波器假设一个系统当前时刻的状态是由上一时刻的状态和输入转化过来的，当然，这个过程中包含着噪声；我们可以使用传感器测量这个状态，但是测量过程也有噪声。那么，该系统可以如下定义：
现在的情况是已经有了测量值z(k)，想要估计系统的真实状态x(k)。
三、MATLAB代码
为了方便第一次实现，首先是一维（仅高度）的情况
% GPS精度为1m、气压计精度为0.5m，加速度计的精度为 1cm/s^2
% 无人机按照螺旋线飞行，半径为 20m，螺距为40m，100s完成一圈飞行
% 数据采集频率为 10Hzclear allD = 1; % 维度，1dt = 0.1; % 0.1s采集一次数据
t0 = 0:dt:100; % 0~100s
N = length(t0); % 采样点数A = eye(2); % 状态转移矩阵
x = zeros(D, N); % 存储滤波后的数据
z = ones(D, N); % 存储滤波前的数据
x(:, 1) = ones(D,1); % 初始值设为 1（可为任意数）
P = eye(D); % 初始值为 1（可为非零任意数），取 D阶单位矩阵Q = 0.01*eye(D); % 过程噪声协方差，估计一个
R = 1; % 测量噪声协方差，精度为多少取多少k = 1; % 采样点计数true1D = t0*0.4; % 一维仅高度，气压计数据for t = dt:dt:100k = k + 1; x(:,k) = A * x(:,k-1) ; % 卡尔曼公式1P = A * P * A' + Q; % 卡尔曼公式2H = eye(D);K = P*H' * inv(H*P*H' + R); % 卡尔曼公式3 z(:,k) = true1D(k) + randn; % 一维仅高度（z方向） x(:,k) = x(:,k) + K * (z(:,k)-H*x(:,k)); % 卡尔曼公式4P = (eye(D)-K*H) * P; % 卡尔曼公式5
endplot(t0, z,'b.'); % 绘制滤波前数据
axis('equal');hold on;grid on; % 坐标等距、继续绘图、添加网格
plot(t0, x,'r.'); % 绘制滤波后数据
plot(t0, true1D ,'k-'); % 绘制真实值
legend('滤波前','滤波后','理想值'); % 标注
xlabel('时间: s');
ylabel('高度: m');hold off;
下面是1~3维都可以使用的代码，其实添加的内容不多，跟一维很类似，只不过多了几个条件判断
% GPS精度为1m、气压计精度为0.5m，加速度计的精度为 1cm/s^2
% 无人机按照螺旋线飞行，半径为 20m，螺距为40m，100s完成一圈飞行
% 数据采集频率为 10Hzclear allD = 3; % 维度，可取 1,2,3dt = 0.1; % 0.1s采集一次数据
t0 = 0:dt:100; % 0~100s
N = length(t0); % 采样点数A = eye(D); % 状态转移矩阵，和上一时刻状态没有换算，故取 D阶单位矩阵
x = zeros(D, N); % 存储滤波后的数据
z = ones(D, N); % 存储滤波前的数据
x(:, 1) = ones(D,1); % 初始值设为 1（可为任意数）
P = eye(D); % 初始值为 1（可为非零任意数），取 D阶单位矩阵r = 20; % 绕圈半径，20m
w = 2*pi / 100; % 计算出角速度，100s绕一圈Q = 1e-2*eye(D); % 过程噪声协方差，估计一个
R = [1 0 0;0 1 0;0 0 0.5]; % 测量噪声协方差，精度为多少取多少k = 1; % 采样点计数if D==1 % 一维仅高度，气压计数据true1D = t0*0.4;
elseif D==2 % 二维 x,y 方向，GPS数据true2D = [r*cos(w*t0); r*sin(w*t0)];
elseif D==3 % 三维 x,y,z方向，GPS和气压计true3D = [r * cos(w*t0); r * sin(w*t0); t0 * 0.4];
endfor t = dt:dt:100k = k + 1; x(:,k) = A * x(:,k-1); % 卡尔曼公式1P = A * P * A' + Q; % 卡尔曼公式2H = eye(D);K = P*H' * inv(H*P*H' + R); % 卡尔曼公式3if D==1 % 一维仅高度（z方向）z(:,k) = true1D(k) + randn; elseif D==2 % 二维 x,y 方向z(:,k) = [true2D(1,k) + randn; true2D(2,k) + randn];elseif D==3 % 三维 x,y,z 方向z(:,k) = [true3D(1,k) + randn; true3D(2,k) + randn; true3D(3,k) + randn];endx(:,k) = x(:,k) + K * (z(:,k)-H*x(:,k)); % 卡尔曼公式4P = (eye(D)-K*H) * P; % 卡尔曼公式5
endif D==1 %% 一维情况plot(t0, z,'b.'); % 绘制滤波前数据axis('equal');hold on;grid on; % 坐标等距、继续绘图、添加网格plot(t0, x,'r.'); % 绘制滤波后数据plot(t0, true1D ,'k-'); % 绘制真实值legend('滤波前','滤波后','理想值'); % 标注xlabel('时间: s'); ylabel('高度: m');hold off;
elseif D==2 %% 二维情况plot(z(1,:),z(2,:),'b.'); % 绘制滤波前数据axis('equal');grid on;hold on; % 坐标等距、继续绘图、添加网格plot(x(1,:),x(2,:),'r.'); % 绘制滤波后数据plot(true2D(1,:), true2D(2,:), 'k.'); % 绘制真实值legend('滤波前','滤波后','理想值'); xlabel('x方向: m');ylabel('y方向: m');hold off;
elseif D==3 %% 三维情况plot3(z(1,:),z(2,:),z(3,:),'b.'); % 绘制滤波前数据axis('equal');grid on;hold on % 坐标等距、继续绘图、添加网格plot3(x(1,:),x(2,:),x(3,:),'r.'); % 绘制滤波后数据plot3(true3D(1,:), true3D(2,:), true3D(3,:));% 绘制滤波后数据legend('滤波前','滤波后','理想值'); % 绘制真实值xlabel('x方向: m');ylabel('y方向: m');zlabel('高度: m');hold off;
end代码中，可以改变 D 的值查看不同维度情况。其实，在2维或3维情况下，各个维度完全是相互独立的，不支持矩阵运算时（比如C语言直接写），分别算几次就好。
出了一期教程，卡尔曼滤波器原理与实战，包括位置估计、姿态估计、推导，腾讯课堂链接卡尔曼滤波器原理与实战-学习视频教程-腾讯课堂
欢迎试听、购买支持。
卡尔曼滤波器MATLAB实现（从一维到三维）相关推荐
- 卡尔曼滤波器求速度matlab,卡尔曼滤波器算法浅析及matlab实战
原标题:卡尔曼滤波器算法浅析及matlab实战 作者:Liu_LongPo 出处:Liu_LongPo的博客 卡尔曼滤波器是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的 ...
- 【Matlab】扩展卡尔曼滤波器原理及仿真（初学者入门专用）
文章目录 0.引言及友情链接 1.场景预设 2.扩展卡尔曼滤波器 3.仿真及效果 0.引言及友情链接 \qquad卡尔曼滤波器(Kalman Filter, KF)是传感器融合(Sensor Fusi ...
- 【MATLAB】卡尔曼滤波器的原理及仿真（初学者专用）
文章目录 0.引言 1.场景预设 2.卡尔曼滤波器 3.仿真及效果 0.引言 \qquad 本文参考了Matlab对卡尔曼滤波器的官方教程及帮助文档(Kalman Filter).官方教程的B站链接如 ...
- 【目标定位】基于matlab扩展卡尔曼滤波器多机器人定位【含Matlab源码 2327期】
⛄一.简介 1 机器人运动模型分析 1.2 坐标系 一个是全局坐标系(Xw,Yw), 另一个是机器人的局部坐标系(Xr,Yr). 如下图所示: 图1 机器人坐标系 室内环境用二维平面表示在全局坐标系X ...
- Udacity机器人软件工程师课程笔记（三十二) - 卡尔曼滤波器 - 一维卡尔曼滤波器 - 多维卡尔曼滤波器 - 拓展卡尔曼滤波器（EKF）
卡尔曼滤波器 一.概述 二.一维高斯分布 均值和方差 三.一维卡尔曼滤波器 变量命名约定 卡尔曼滤波循环 1.测量值更新 (1)平均值计算 (2)程序实现 2.位置预测 位置预测公式 3.一维卡尔曼滤 ...
- matlab中卡尔曼滤波,卡尔曼滤波器和matlab代码.doc
完美WORD格式 专业整理分享 信息融合大作业 --维纳最速下降法滤波器,卡尔曼滤波器设计及Matlab仿真 时间:2010-12-6 专业:信息工程 班级学号:2007302171 姓名:马志强 滤 ...
- 基于MATLAB的RSSI 和 PLE 定位算法，并通过卡尔曼滤波器减少非视距误差
基于MATLAB的RSSI 和 PLE 定位算法,并通过卡尔曼滤波器减少非视距误差 根据上面的课题要求,我们知道在室内未知信道环境下,进行RSS估计效果较差,而本课题所要求的是在室内未知PL的情况下进 ...
- 卡尔曼滤波器原理和matlab实现
项目最近正好用上kalman滤波器,故整理一下kalman滤波器相关资料,网上有很多详细的kalman资料,参考如下: 1.https://zhuanlan.zhihu.com/p/34656822 ...
- 【信号处理】基于扩展卡尔曼滤波器和无迹卡尔曼滤波器的窄带信号时变频率估计（Matlab代码实现）
目录 1 概述 2 数学模型 3 运行结果 4 结论 5 参考文献 6 Matlab代码实现 1 概述 本文讲解和比较了基于卡尔曼滤波器的频率跟踪方法的能力,例如扩展卡尔曼滤波器 (EKF) 和无味卡 ...
- 西门子s7300 C MATLAB,卡尔曼滤波器在s7-300系列plc中的实现方法
卡尔曼滤波器在s7-300系列plc中的实现方法 [专利摘要]本发明公开了一种卡尔曼滤波器在S7-300系列PLC中的实现方法.在S7-300系列PLC系统中定义16个数据块,其中的14个数据块和卡尔 ...
最新文章
- Qt学习一门：直接使用QT具
- python100个必背知识-python学习必备知识汇总
- C0304 备份最后一天所有修改的文件
- 搜狗手机浏览器_搜狗推广开户费多少钱？【搜狗开户代理商】
- c#中属性自动完成快捷键
- pom.xml 配置maven私服
- Eclipse-Git
- 杭电HDUacm2098
- SAP GUI 770下载
- 【开发神器】自动化测试、用 Apipost！
- mdx格式的词典用什么软件打开_英专生必备词典
- Qt 之 QQ系统表情（五）
- 锂电池技术关键突破：水淹火烧重击短路都不炸！三星看了会沉默，特斯拉蔚来听了要流泪...
- 盗贼之海服务器维护时间,盗贼之海常见问题解决方法
- 正则表达式课后练习题
- SEO优化与SEM（竞价）区别在哪?
- Mac版DBeaver调整编辑窗口字体大小
- 20、Java——迷你图书管理器（对象+集合）
- 软件工程网络15个人阅读作业2（201521123042 姚佳希）
- C语言基础：折半查找