设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 嵌入式/单片机论坛 › MCU综合技术区 › GPS和INS联合导航matlab程序（带数据有用的）
返回列表 发新帖
查看: 5754|回复: 2
打印 上一主题 下一主题
收起左侧

GPS和INS联合导航matlab程序（带数据有用的）

[复制链接]
跳转到指定楼层
楼主
ID:354298 发表于 2018-6-19 15:06 | 只看该作者 |只看大图 回帖奖励 |倒序浏览 |阅读模式
本压缩文件是组合导航常用的算法
组合导航，带有数据，可以直接用

全部资料51hei下载地址：
GPS和INS联合导航matlab程序（带数据有用的）.zip (620.09 KB, 下载次数: 98)


  1. %GPS/INS无反馈位置组合 卡尔曼滤波器

  3. %%%%%%%%%%%%%%%%%%
  4. %edit by horsejun
  5. %%%%%%%%%%%%%%%%%%

  7. %每秒更新一次速度位置误差
  8. %连续状态系统方程
  9. %dx = F*x + G*w
  10. %z = H*x + v
  11. %离散状态系统方程
  12. %x(k+1) = A*x(k) + B*w(k)
  13. %z(k+1) = C*x(k+1) + v(k+1)

  15. function [E_attitude, E_velocity, E_position, PP] = kalman_GPS_INS_position_sp_NFb(Dp, v, p, quat, Fn, Q, R, Tg, Ta, tao)

  17. %输入
  18. %Dp     量测位置误差， 作为滤波器输入,
  19. %Dv     量测速度误差， 作为滤波器输入，
  20. %p      ins输出位置，作为滤波器系统参数
  21. %v      ins输出速度，作为滤波器系统参数
  22. %fn     ins输出导航系下比力，作为滤波器参数
  23. %quat   ins输出四元数，作为滤波器参数
  24. %Q      系统噪声方差
  25. %R      测量噪声方差
  26. %Ta     加表误差漂移相关时间
  27. %Tg     陀螺仪误差漂移相关时间
  28. %tao    迭代步长
  29. %%%%%%%输入向量均为行向量%%%%%%%%%%%%%

  31. %输出
  32. %E_position     位置预测值
  33. %E-velocity     速度预测值

  35. %各参数初始化
  36. Re                 = 6378245;   %地球长半径
  37. e                 = 1/298.257;  %地球扁率
  38. wie         = 7.292e-5;  %地球自转角速度

  40. %   东北天速度
  41. Ve0   = v(:,1);
  42. Vn0   = v(:,2);
  43. Vu0   = v(:,3);
  44. %   导航位置
  45. L0    = p(:,1);
  46. h0    = p(:,3);

  48. %卡尔曼滤波参数初始化
  49. PP(1:18,1:18) = diag([1/(36*57) 1/(36*57) 1/57, 0.0001 0.0001 0.0001, 0 0 1, 0.1/(57*3600) 0.1/(57*3600) 0.1/(57*3600), 0.04/(57*3600) 0.04/(57*3600) 0.04/(57*3600), 1e-4 1e-4 1e-4].^2);   %初始误差协方差阵
  50. PP0                                         = PP;
  51. X                                                 = zeros(18,1);  %初始状态
  52. E_attitude                 = zeros(1,3);
  53. E_position                 = zeros(1,3);
  54. E_velocity                 = zeros(1,3);

  56. n = size(Dp,1);
  57. for i=1:n-1
  58.     %参数赋值
  59.     Ve                 = Ve0(i);
  60.     Vn                 = Vn0(i);
  61.     Vu                 = Vu0(i);
  62.     L                 = L0(i);
  63.     h                 = h0(i);
  64.     fe                 = Fn(i,1);
  65.     fn                 = Fn(i,2);
  66.     fu                 = Fn(i,3);
  67.     Rm                 = Re*(1-2*e+3*e*sin(L)^2);
  68.     Rn                 = Re*(1-e*sin(L)^2);
  69.     %由四元数计算姿态阵
  70.     q                 = quat(i,:);
  71.     Cnb         = [1-2*(q(3)^2+q(4)^2),     2*(q(2)*q(3)-q(1)*q(4)), 2*(q(2)*q(4)+q(1)*q(3));
  72.                     2*(q(2)*q(3)+q(1)*q(4)), 1-2*(q(2)^2+q(4)^2),     2*(q(3)*q(4)-q(1)*q(2));
  73.              2*(q(2)*q(4)-q(1)*q(3)), 2*(q(3)*q(4)+q(1)*q(2)), 1-2*(q(2)^2+q(3)^2)];

  75.     %连续系统状态转换阵 F 的时间更新
  76.     F            = zeros(18,18);
  77.     F(1,2)       = wie*sin(L)+Ve*tan(L)/(Rn+h);
  78.     F(1,3)       = -(wie*cos(L)+Ve/(Rn+h));
  79.     F(1,5)       = -1/(Rm+h);
  80.     F(1,9)       = Vn/(Rm+h)^2;
  81.     F(2,1)       = -(wie*sin(L)+Ve*tan(L)/(Rn+h));
  82.     F(2,3)       = -Vn/(Rm+h);
  83.     F(2,4)       = 1/(Rn+h);
  84.     F(2,7)       = -wie*sin(L);
  85.     F(2,9)       = -Ve/(Rn+h)^2;
  86.     F(3,1)       = wie*cos(L)+Ve/(Rn+h);
  87.     F(3,2)       = Vn/(Rm+h);
  88.     F(3,4)       = tan(L)/(Rn+h);
  89.     F(3,7)       = wie*cos(L)+Ve*(sec(L)^2)/(Rn+h);
  90.     F(3,9)       = -Ve*tan(L)/(Rn+h)^2;
  91.     F(4,2)       = -fu;
  92.     F(4,3)       = fn;
  93.     F(4,4)       = Vn*tan(L)/(Rm+h)-Vu/(Rm+h);
  94.     F(4,5)       = 2*wie*sin(L)+Ve*tan(L)/(Rn+h);
  95.     F(4,6)       = -(2*wie*cos(L)+Ve/(Rn+h));
  96.     F(4,7)       = 2*wie*cos(L)*Vn+Ve*Vn*sec(L)^2/(Rn+h)+2*wie*sin(L)*Vu;
  97.     F(4,9)       = (Ve*Vu-Ve*Vn*tan(L))/(Rn+h)^2;
  98.     F(5,1)       = fu;
  99.     F(5,3)       = -fe;
  100.     F(5,4)       = -2*(wie*sin(L)+Ve*tan(L)/(Rn+h));
  101.     F(5,5)       = -Vu/(Rm+h);
  102.     F(5,6)       = -Vn/(Rm+h);
  103.     F(5,7)       = -(2*wie*cos(L)+Ve*(sec(L)^2)/(Rn+h))*Ve;
  104.     F(5,9)       = (Ve^2*tan(L)+Vn*Vu)/(Rn+h)^2;
  105.     F(6,1)       = -fn;
  106.     F(6,2)       = fe;
  107.     F(6,4)       = 2*(wie*cos(L)+Ve/(Rn+h));
  108.     F(6,5)       = 2*Vn/(Rm+h);
  109.     F(6,7)       = -2*Ve*wie*sin(L);
  110.     F(6,9)       = -(Vn^2+Ve^2)/(Rn+h)^2;
  111.     F(7,5)       = 1/(Rm+h);
  112.     F(7,9)       = -Vn/(Rm+h)^2;
  113.     F(8,4)       = 1/((Rn+h)*cos(L));
  114.     F(8,7)       = Ve*tan(L)/((Rn+h)*cos(L));
  115.     F(8,9)       = -Ve/(cos(L)*(Rn+h)^2);
  116.     F(9,6)       = 1;
  117.     F(1:3,10:12) = Cnb;
  118.     F(1:3,13:15) = Cnb;
  119.     F(4:6,16:18) = Cnb;
  120.     F(13,13)     = -1/Tg(1);
  121.     F(14,14)     = -1/Tg(2);
  122.     F(15,15)     = -1/Tg(3);
  123.     F(16,16)     = -1/Ta(1);
  124.     F(17,17)     = -1/Ta(2);
  125.     F(18,18)     = -1/Ta(3);
  126.     %连续系统输入矩阵更新
  127.     G                                    = zeros(18,9);
  128.     G(1:3,1:3)   = Cnb;
  129.     G(13:15,4:6) = eye(3,3);
  130.     G(16:18,7:9) = eye(3,3);
  131.     %连续系统量测阵更新
  132.     H                                         = zeros(3,18);
  133.     H(1,7)                         = 1;
  134.     H(2,8)                         = 1;
  135.     H(3,9)                         = 1;
  136.     %连续系统离散化
  137.     A                                   = eye(18,18)+F*tao;
  138.     B                             = (eye(18,18)+tao*F/2)*G*tao;
  139.    
  140.     %卡尔曼滤波
  141.     P                                         = A*(PP0)*A'+B*Q*B';
  142.     K                                         = P*H'*inv(H*P*H'+R);
  143.     PP0                                 = (eye(18,18)-K*H)*P;
  144.     PP0                                 = (PP0+PP0')/2;
  145.     PP(i,:)                 = diag(PP0);
  146.    
  147.     z = Dp(i+1,:)';
  148.     XX = A*X+K*(z-H*A*X);
  149.     X = XX;

  151.     E_attitude(i+1,:) = XX(1:3)';
  152.     E_velocity(i+1,:) = XX(4:6)';
  153.     E_position(i+1,:) = XX(7:9)';
  154. end

复制代码
  1. %GPS/INS组合导航

  3. %%%%%%%%%%%%%%%%%%
  4. %edit by horsejun
  5. %%%%%%%%%%%%%%%%%%

  7. %量测信号：   位置
  8. %INS输出数据由simulink计算得出


  11. clear
  12. clc

  14. %得到轨迹信号

  16. load ode500

  18. Re                 = 6378245;                                                                                                                                                                                     %地球长半径

  20. %真实轨迹
  21. a_R         = yout(:,1:3);
  22. v_R          = yout(:,4:6);
  23. p_R         = yout(:,7:9);

  25. %加噪声后的INS计算结果
  26. a_ins = yout(:,10:12);
  27. v_ins = yout(:,13:15);
  28. p_ins = yout(:,16:18);
  29. quat  = yout(:,19:22);    %姿态四元数
  30. Fn    = yout(:,23:25);         %地理系下的比力

  32. %惯导相关的噪声统计数据
  33. Q_wg  = (0.04/(57*3600))^2;         %陀螺马氏过程
  34. Q_wr  = (0.01/(57*3600))^2;              %陀螺白噪声
  35. Q_wa  = (1e-3)^2;   %加计马氏过程
  36. Q                 = diag([Q_wg Q_wg Q_wg,  Q_wr Q_wr Q_wr,  Q_wa Q_wa Q_wa]);
  37. Tg                 = 300*ones(3,1);
  38. Ta                 = 1000*ones(3,1);

  40. %得到带误差的GPS输出信号
  41. p_gps_sample = p_R(1:10:end,:);
  42. n = size(p_gps_sample,1);
  43. p_error(:,1:2)  = 30*randn(n,2)/Re;   
  44. p_error(:,3) = 30*randn(n,1);                                %位置误差
  45. p_gps = p_gps_sample+p_error;                                %加入位置误差
  46. R = diag(std(p_error).^2);                                   %计算测量噪声方差R

  48. %卡尔曼滤波
  49. tao= 1;                                            %滤波步长
  50. a_ins_sample                 = a_ins(1:10:end,:);
  51. v_ins_sample                 = v_ins(1:10:end,:);
  52. p_ins_sample                 = p_ins(1:10:end,:);
  53. a_R_sample                         = a_R(1:10:end,:);
  54. v_R_sample                         = v_R(1:10:end,:);
  55. p_R_sample                         = p_R(1:10:end,:);
  56. Dp= p_ins_sample-p_gps;                           %INS与GPS输出的位置差值
  57. a = a_ins_sample;
  58. v = v_ins_sample;
  59. p = p_ins_sample;
  60. quat0 = quat(1:10:end,:);
  61. Fn0 = Fn(1:10:end,:);

  63. [Error_a, Error_v, Error_p, PP] = kalman_GPS_INS_position_sp_NFb(Dp, v, p, quat0, Fn0, Q, R, Tg, Ta, tao);   %得到位置，速度误差误差估计值

  65. a_estimate                         = a(1:size(Error_a,1),:)-Error_a;
  66. v_estimate                         = v(1:size(Error_v,1),:)-Error_v;
  67. p_estimate                         = p(1:size(Error_p,1),:)-Error_p;

  69. n = size(p_estimate,1);   %行数

  71. %位置误差比较
  72. figure  
  73. subplot(3,1,1)
  74. plot((1:n),(p_R_sample(1:n,1)-p(1:n,1))*6e6,'k',(1:n),(p_R_sample(1:n,1)-p_estimate(:,1))*6e6,'r')  %黑线-滤波前的误差   红线-滤波后的误差
  75. xlabel('时间，单位s')
  76. subplot(3,1,2)
  77. plot((1:n),(p_R_sample(1:n,2)-p(1:n,2))*6e6,'k',(1:n),(p_R_sample(1:n,2)-p_estimate(:,2))*6e6,'r')  %黑线-滤波前的误差   红线-滤波后的误差
  78. ylabel('位置误差，单位m')
  79. subplot(3,1,3)
  80. plot((1:n),p_R_sample(1:n,3)-p(1:n,3),'k',(1:n),p_R_sample(1:n,3)-p_estimate(:,3),'r')                                                  %黑线-滤波前的误差   红线-滤波后的误差
  81. xlabel('黑线-滤波前的INS误差   红线-滤波后的误差')

  83. %速度误差比较
  84. figure  
  85. subplot(3,1,1)
  86. plot((1:n),v_R_sample(1:n,1)-v(1:n,1),'k',(1:n),v_R_sample(1:n,1)-v_estimate(:,1),'r')                                                   %黑线-滤波前的误差   红线-滤波后的误差
  87. xlabel('时间，单位s')
  88. subplot(3,1,2)
  89. plot((1:n),v_R_sample(1:n,2)-v(1:n,2),'k',(1:n),v_R_sample(1:n,2)-v_estimate(:,2),'r')                                                  %黑线-滤波前的误差   红线-滤波后的误差
  90. ylabel('速度误差，单位m/s')
  91. subplot(3,1,3)
  92. plot((1:n),v_R_sample(1:n,3)-v(1:n,3),'k',(1:n),v_R_sample(1:n,3)-v_estimate(:,3),'r')                                                  %黑线-滤波前的误差   红线-滤波后的误差
  93. xlabel('黑线-滤波前的INS误差   红线-滤波后的误差')

  95. %位置误差
  96. figure  
  97. subplot(3,1,1)
  98. xlabel('时间，单位s')
  99. plot((1:n),(p_R_sample(1:n,1)-p_estimate(:,1))*6370000,'r')                                                                                                                                                                  %红线-滤波后的误差
  100. subplot(3,1,2)
  101. plot((1:n),(p_R_sample(1:n,2)-p_estimate(:,2))*6370000,'r')                                                                                                                                                                  %红线-滤波后的误差
  102. ylabel('位置误差，单位m')
  103. subplot(3,1,3)
  104. plot((1:n),p_R_sample(1:n,3)-p_estimate(:,3),'r')                                                                                                                                                                                                    %红线-滤波后的误差
  105. xlabel('滤波后的位置误差')

  107. %速度误差
  108. figure  
  109. subplot(3,1,1)
  110. plot((1:n),v_R_sample(1:n,1)-v_estimate(:,1),'r')                                                                                                                                                                                                          % 红线-滤波后的误差
  111. xlabel('时间，单位s')
  112. subplot(3,1,2)
  113. plot((1:n),v_R_sample(1:n,2)-v_estimate(:,2),'r')                                                                                                                                                                                                          %红线-滤波后的误差
  114. ylabel('速度误差，单位m/s')
  115. subplot(3,1,3)
  116. plot((1:n),v_R_sample(1:n,3)-v_estimate(:,3),'r')                                                                                                                                                                                                          % 红线-滤波后的误差
  117. xlabel('滤波后的速度误差')

复制代码



分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏1 分享淘帖 顶顶 踩踩
回复

使用道具 举报

沙发
ID:508323 发表于 2019-4-9 22:00 | 只看该作者
顶帖，很赞
回复

使用道具 举报

板凳
ID:809854 发表于 2020-7-30 21:32 | 只看该作者
又看到您了
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表