在好例子网,分享、交流、成长!
您当前所在位置:首页MATLAB 开发实例MATLAB语言基础 → Matlab轨迹预测

Matlab轨迹预测

MATLAB语言基础

下载此实例
  • 开发语言:MATLAB
  • 实例大小:5.28KB
  • 下载次数:16
  • 浏览次数:196
  • 发布时间:2021-10-16
  • 实例类别:MATLAB语言基础
  • 发 布 人:anyone007
  • 文件格式:.m
  • 所需积分:2
 相关标签: 预测 轨迹

实例介绍

【实例简介】

使用Matlab实现卡尔曼滤波的轨迹预测

【实例截图】

from clipboard

【核心代码】


% 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



标签: 预测 轨迹

实例下载地址

Matlab轨迹预测

不能下载?内容有错? 点击这里报错 + 投诉 + 提问

好例子网口号:伸出你的我的手 — 分享

网友评论

发表评论

(您的评论需要经过审核才能显示)

查看所有0条评论>>

小贴士

感谢您为本站写下的评论,您的评论对其它用户来说具有重要的参考价值,所以请认真填写。

  • 类似“顶”、“沙发”之类没有营养的文字,对勤劳贡献的楼主来说是令人沮丧的反馈信息。
  • 相信您也不想看到一排文字/表情墙,所以请不要反馈意义不大的重复字符,也请尽量不要纯表情的回复。
  • 提问之前请再仔细看一遍楼主的说明,或许是您遗漏了。
  • 请勿到处挖坑绊人、招贴广告。既占空间让人厌烦,又没人会搭理,于人于己都无利。

关于好例子网

本站旨在为广大IT学习爱好者提供一个非营利性互相学习交流分享平台。本站所有资源都可以被免费获取学习研究。本站资源来自网友分享,对搜索内容的合法性不具有预见性、识别性、控制性,仅供学习研究,请务必在下载后24小时内给予删除,不得用于其他任何用途,否则后果自负。基于互联网的特殊性,平台无法对用户传输的作品、信息、内容的权属或合法性、安全性、合规性、真实性、科学性、完整权、有效性等进行实质审查;无论平台是否已进行审查,用户均应自行承担因其传输的作品、信息、内容而可能或已经产生的侵权或权属纠纷等法律责任。本站所有资源不代表本站的观点或立场,基于网友分享,根据中国法律《信息网络传播权保护条例》第二十二与二十三条之规定,若资源存在侵权或相关问题请联系本站客服人员,点此联系我们。关于更多版权及免责申明参见 版权及免责申明

;
报警