实例介绍
【实例简介】
使用Matlab实现卡尔曼滤波的轨迹预测
【实例截图】
【核心代码】
% GPS精度为1m、气压计精度为0.5m,加速度计的精度为 1cm/s^2
% 无人机按照螺旋线飞行,半径为 20m,螺距为40m,100s完成一圈飞行
% 数据采集频率为 10Hz
clear all
D = 1; % 维度,1
dt = 0.1; % 0.1s采集一次数据
t0 = 0:dt:100; % 0~100s
N = length(t0); % 采样点数
A = eye(D); % 状态转移矩阵,和上一时刻状态没有换算,故取 D阶单位矩阵,eye(1), 1阶单位阵 = 1
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 = 4; % 采样点计数
true1D = t0*0.4; % 一维仅高度,气压计数据
for t = dt:dt:100
k = k 1;
x(:,k) = A * x(:,k-1); % 卡尔曼公式1
P = A * P * A' Q; % 卡尔曼公式2
H = 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)); % 卡尔曼公式4
P = (eye(D)-K*H) * P; % 卡尔曼公式5
end
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;
下面是1~3维都可以使用的代码,其实添加的内容不多,跟一维很类似,只不过多了几个条件判断
% GPS精度为1m、气压计精度为0.5m,加速度计的精度为 1cm/s^2
% 无人机按照螺旋线飞行,半径为 20m,螺距为40m,100s完成一圈飞行
% 数据采集频率为 10Hz
clear all
D = 3; % 维度,可取 1,2,3
dt = 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];
end
for t = dt:dt:100
k = k 1;
x(:,k) = A * x(:,k-1); % 卡尔曼公式1
P = A * P * A' Q; % 卡尔曼公式2
H = eye(D);
K = P*H' * inv(H*P*H' R); % 卡尔曼公式3
if 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];
end
x(:,k) = x(:,k) K * (z(:,k)-H*x(:,k)); % 卡尔曼公式4
P = (eye(D)-K*H) * P; % 卡尔曼公式5
end
if 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
小贴士
感谢您为本站写下的评论,您的评论对其它用户来说具有重要的参考价值,所以请认真填写。
- 类似“顶”、“沙发”之类没有营养的文字,对勤劳贡献的楼主来说是令人沮丧的反馈信息。
- 相信您也不想看到一排文字/表情墙,所以请不要反馈意义不大的重复字符,也请尽量不要纯表情的回复。
- 提问之前请再仔细看一遍楼主的说明,或许是您遗漏了。
- 请勿到处挖坑绊人、招贴广告。既占空间让人厌烦,又没人会搭理,于人于己都无利。
关于好例子网
本站旨在为广大IT学习爱好者提供一个非营利性互相学习交流分享平台。本站所有资源都可以被免费获取学习研究。本站资源来自网友分享,对搜索内容的合法性不具有预见性、识别性、控制性,仅供学习研究,请务必在下载后24小时内给予删除,不得用于其他任何用途,否则后果自负。基于互联网的特殊性,平台无法对用户传输的作品、信息、内容的权属或合法性、安全性、合规性、真实性、科学性、完整权、有效性等进行实质审查;无论平台是否已进行审查,用户均应自行承担因其传输的作品、信息、内容而可能或已经产生的侵权或权属纠纷等法律责任。本站所有资源不代表本站的观点或立场,基于网友分享,根据中国法律《信息网络传播权保护条例》第二十二与二十三条之规定,若资源存在侵权或相关问题请联系本站客服人员,点此联系我们。关于更多版权及免责申明参见 版权及免责申明
网友评论
我要评论