亚洲春色中文字幕久久久-三上亚,一吻二脱三床四吻胸,国产真实伦对白视频全集,在线毛片观看,精品成品入口黄网,国产毛aⅴ片久久久,亚洲AV色香蕉一区二区三区老师,萧皇后A级艳片,色情日本视频更新,99久久亚洲精品日本无码

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

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

運行結(jié)果：


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

  11. clear all;

  13. %% 慣性-GPS組合導(dǎo)航模型參數(shù)初始化
  14. we  = 360/24/60/60*pi/180; %地球自轉(zhuǎn)角速度，弧度/s
  15. psi = 10*pi/180;          %psi角度 / 弧度
  16. Tge = 0.12;
  17. Tgn = 0.10;
  18. Tgz = 0.10;               %這三個參數(shù)的含義詳見參考文獻(xiàn)
  19. sigma_ge=1;
  20. sigma_gn=1;
  21. sigma_gz=1;
  22. %% 連續(xù)空間系統(tǒng)狀態(tài)方程
  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;]; %狀態(tài)轉(zhuǎn)移矩陣
  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. %% 轉(zhuǎn)化為離散時間系統(tǒng)狀態(tài)方程
  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. %% 卡爾曼濾波器參數(shù)初始化
  44. t=0:T:50-T;
  45. length=size(t,2);
  46. y=zeros(2,length);
  47. Q=0.5^2*eye(3);                %系統(tǒng)噪聲協(xié)方差
  48. R=0.25^2*eye(2);               %測量噪聲協(xié)方差
  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);             %狀態(tài)估計值，9維
  53. X(:,1)=[0,0,0,0,0,0,0,0,0]';   %狀態(tài)估計初始值設(shè)定
  54. P=eye(9);                      %狀態(tài)估計協(xié)方差
  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('濾波后的觀測量');
復(fù)制代碼




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