久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

 找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

搜索
查看: 6427|回復: 2
打印 上一主題 下一主題
收起左側

GPS和INS聯合導航matlab程序（帶數據有用的）

[復制鏈接]
跳轉到指定樓層
樓主
本壓縮文件是組合導航常用的算法
組合導航，帶有數據，可以直接用

全部資料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 | 只看該作者
又看到您了
回復

使用道具 舉報

您需要登錄后才可以回帖 登錄 | 立即注冊

本版積分規則

手機版|小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術交流QQ群281945664

Powered by 單片機教程網

快速回復 返回頂部 返回列表
主站蜘蛛池模板： 密室大逃脱第六季大神版在线观看 | 欧美中文字幕一区 | 免费国产黄网站在线观看视频 | 欧美精品一 | 99爱在线观看 | 久久精品日产第一区二区三区 | 七七婷婷婷婷精品国产 | 久久机热 | 99re视频| 国产欧美精品一区二区色综合朱莉 | 国产精品久久777777 | 超碰综合 | 久草网址| 日韩中文字幕在线视频 | 亚洲91| 色伊人 | 日韩中文字幕在线播放 | h视频免费在线观看 | 久久久久亚洲av毛片大全 | 99热在线播放 | 日韩福利视频 | 夜夜久久 | 亚洲国产一区二区三区在线观看 | 99久久久久久 | 99国产精品视频免费观看一公开 | 日韩精品一区二区三区视频播放 | 在线观看a视频 | 免费三级av | 欧美一区二区二区 | 一区二区三区回区在观看免费视频 | 久久久激情 | 美国av毛片 | 国产精品美女一区二区 | 91在线观看 | 天天操夜夜艹 | 国产丝袜一区二区三区免费视频 | 国产一级电影在线 | 在线视频日韩 | 一呦二呦三呦国产精品 | 91久久国产综合久久91精品网站 | 精品一区二区三区在线观看 |