欧美极品高清xxxxhd,国产日产欧美最新,无码AV国产东京热AV无码,国产精品人与动性XXX,国产传媒亚洲综合一区二区,四库影院永久国产精品,毛片免费免费高清视频,福利所导航夜趣136

標題: 卡爾曼濾波器在INS-GPS組合導航中應用仿真 matlab源代碼 [打印本頁]
作者: redcircle052173    時間: 2021-12-9 09:04
標題: 卡爾曼濾波器在INS-GPS組合導航中應用仿真 matlab源代碼
最近一段時間在嘗試用FPGA設計卡爾曼濾波器，設計過程比較復雜，在此過程中卡爾曼濾波器的基本原理，matlab仿真與C語言實現，及其在導航算法中的應用困擾了我很長一段時間。在和師兄交流和參閱些許文獻之后，終于小有收獲。下面是把卡爾曼濾波器在INS-GPS組合導航中應用仿真的matlab源代碼與大家分享，希望對初學者有一定的借鑒價值。

   在下面的仿真的代碼中，理想的觀測量不是真實數據，而是自生成的正弦波數據，在真實的應用場景中，應該是一系列的參考數據。

運行結果：


  1. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2. %     卡爾曼濾波器在INS-GPS組合導航中應用仿真
  3. % Author : lylogn
  4. % Email  : lylogn@gmail.com
  5. % Company: BUAA-Dep3
  6. % Time   : 2013.01.06
  7. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  8. % 參考文獻:
  9. % [1]. 鄧正隆. 慣導技術, 哈爾濱工業大學出版社.2006.

  11. clear all;

  13. %% 慣性-GPS組合導航模型參數初始化
  14. we  = 360/24/60/60*pi/180; %地球自轉角速度，弧度/s
  15. psi = 10*pi/180;          %psi角度 / 弧度
  16. Tge = 0.12;
  17. Tgn = 0.10;
  18. Tgz = 0.10;               %這三個參數的含義詳見參考文獻
  19. sigma_ge=1;
  20. sigma_gn=1;
  21. sigma_gz=1;
  22. %% 連續空間系統狀態方程
  23. % X_dot(t) = A(t)*X(t) + B(t)*W(t)

  25. A=[0 we*sin(psi) -we*cos(psi) 1     0     0      1 0 0;
  26.    -we*sin(psi) 0    0      0     1     0      0 1 0;
  27.    we*cos(psi) 0    0      0     0     1      0 0 1;
  28.   0           0    0      -1/Tge 0     0      0 0 0;
  29.   0           0    0      0     -1/Tgn 0      0 0 0;
  30.   0           0    0      0     0     -1/Tgz  0 0 0;
  31.   0           0    0      0     0     0      0 0 0;
  32.   0           0    0      0     0     0      0 0 0;
  33.   0           0    0      0     0     0      0 0 0;]; %狀態轉移矩陣
  34. B=[0           0    0   sigma_ge*sqrt(2/Tge) 0 0 0 0 0;
  35.   0           0    0    0 sigma_gn*sqrt(2/Tgn) 0 0 0 0;
  36.   0           0    0    0 0 sigma_gz*sqrt(2/Tgz) 0 0 0;]';%輸入控制矩陣
  37. %% 轉化為離散時間系統狀態方程
  38. % X(k+1) = F*X(k) + G*W(k)
  39. T = 0.1;
  40. [F,G]=c2d(A,B,T);
  41. H=[1   0      0 0 0 0 0 0 0;
  42.    0  -sec(psi) 0 0 0 0 0 0 0;];%觀測矩陣
  43. %% 卡爾曼濾波器參數初始化
  44. t=0:T:50-T;
  45. length=size(t,2);
  46. y=zeros(2,length);
  47. Q=0.5^2*eye(3);                %系統噪聲協方差
  48. R=0.25^2*eye(2);               %測量噪聲協方差
  49. y(1,:)=2*sin(pi*t*0.5);
  50. y(2,:)=2*cos(pi*t*0.5);
  51. Z=y+sqrt(R)*randn(2,length);   %生成的含有噪聲的假定觀測值，2維
  52. X=zeros(9,length);             %狀態估計值，9維
  53. X(:,1)=[0,0,0,0,0,0,0,0,0]';   %狀態估計初始值設定
  54. P=eye(9);                      %狀態估計協方差
  55. %% 卡爾曼濾波算法迭代過程
  56. for  n=2:length
  57.    X(:,n)=F*X(:,n-1);
  58.    P=F*P*F'+ G*Q*G';
  59.    Kg=P*H'/(H*P*H'+R);
  60.   X(:,n)=X(:,n)+Kg*(Z(:,n)-H*X(:,n));
  61.    P=(eye(9,9)-Kg*H)*P;
  62. end
  63. %% 繪圖代碼
  64. figure(1)
  65. plot(y(1,:))
  66. hold on;
  67. plot(y(2,:))
  68. hold off;
  69. title('理想的觀測量');
  70. figure(2)
  71. plot(Z(1,:))
  72. hold on;
  73. plot(Z(2,:))
  74. hold off;
  75. title('帶有噪聲的觀測量');
  76. figure(3)
  77. plot(X(1,:))
  78. hold on;
  79. plot(X(2,:))
  80. hold off;
  81. title('濾波后的觀測量');
復制代碼




歡迎光臨 (http://www.raoushi.com/bbs/) Powered by Discuz! X3.1