|
|
最近一段時(shí)間在嘗試用FPGA設(shè)計(jì)卡爾曼濾波器，設(shè)計(jì)過(guò)程比較復(fù)雜，在此過(guò)程中卡爾曼濾波器的基本原理，matlab仿真與C語(yǔ)言實(shí)現(xiàn)，及其在導(dǎo)航算法中的應(yīng)用困擾了我很長(zhǎng)一段時(shí)間。在和師兄交流和參閱些許文獻(xiàn)之后，終于小有收獲。下面是把卡爾曼濾波器在INS-GPS組合導(dǎo)航中應(yīng)用仿真的matlab源代碼與大家分享，希望對(duì)初學(xué)者有一定的借鑒價(jià)值。
在下面的仿真的代碼中，理想的觀測(cè)量不是真實(shí)數(shù)據(jù)，而是自生成的正弦波數(shù)據(jù)，在真實(shí)的應(yīng)用場(chǎng)景中，應(yīng)該是一系列的參考數(shù)據(jù)。
運(yùn)行結(jié)果：
1639011807(1).png (287.62 KB, 下載次數(shù): 73)
下載附件
2021-12-9 09:04 上傳
- %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- % 卡爾曼濾波器在INS-GPS組合導(dǎo)航中應(yīng)用仿真
- % Author : lylogn
- % Email : lylogn@gmail.com
- % Company: BUAA-Dep3
- % Time : 2013.01.06
- %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- % 參考文獻(xiàn):
- % [1]. 鄧正隆. 慣導(dǎo)技術(shù), 哈爾濱工業(yè)大學(xué)出版社.2006.
- clear all;
- %% 慣性-GPS組合導(dǎo)航模型參數(shù)初始化
- we = 360/24/60/60*pi/180; %地球自轉(zhuǎn)角速度，弧度/s
- psi = 10*pi/180; %psi角度 / 弧度
- Tge = 0.12;
- Tgn = 0.10;
- Tgz = 0.10; %這三個(gè)參數(shù)的含義詳見參考文獻(xiàn)
- sigma_ge=1;
- sigma_gn=1;
- sigma_gz=1;
- %% 連續(xù)空間系統(tǒng)狀態(tài)方程
- % X_dot(t) = A(t)*X(t) + B(t)*W(t)
- A=[0 we*sin(psi) -we*cos(psi) 1 0 0 1 0 0;
- -we*sin(psi) 0 0 0 1 0 0 1 0;
- we*cos(psi) 0 0 0 0 1 0 0 1;
- 0 0 0 -1/Tge 0 0 0 0 0;
- 0 0 0 0 -1/Tgn 0 0 0 0;
- 0 0 0 0 0 -1/Tgz 0 0 0;
- 0 0 0 0 0 0 0 0 0;
- 0 0 0 0 0 0 0 0 0;
- 0 0 0 0 0 0 0 0 0;]; %狀態(tài)轉(zhuǎn)移矩陣
- B=[0 0 0 sigma_ge*sqrt(2/Tge) 0 0 0 0 0;
- 0 0 0 0 sigma_gn*sqrt(2/Tgn) 0 0 0 0;
- 0 0 0 0 0 sigma_gz*sqrt(2/Tgz) 0 0 0;]';%輸入控制矩陣
- %% 轉(zhuǎn)化為離散時(shí)間系統(tǒng)狀態(tài)方程
- % X(k+1) = F*X(k) + G*W(k)
- T = 0.1;
- [F,G]=c2d(A,B,T);
- H=[1 0 0 0 0 0 0 0 0;
- 0 -sec(psi) 0 0 0 0 0 0 0;];%觀測(cè)矩陣
- %% 卡爾曼濾波器參數(shù)初始化
- t=0:T:50-T;
- length=size(t,2);
- y=zeros(2,length);
- Q=0.5^2*eye(3); %系統(tǒng)噪聲協(xié)方差
- R=0.25^2*eye(2); %測(cè)量噪聲協(xié)方差
- y(1,:)=2*sin(pi*t*0.5);
- y(2,:)=2*cos(pi*t*0.5);
- Z=y+sqrt(R)*randn(2,length); %生成的含有噪聲的假定觀測(cè)值，2維
- X=zeros(9,length); %狀態(tài)估計(jì)值，9維
- X(:,1)=[0,0,0,0,0,0,0,0,0]'; %狀態(tài)估計(jì)初始值設(shè)定
- P=eye(9); %狀態(tài)估計(jì)協(xié)方差
- %% 卡爾曼濾波算法迭代過(guò)程
- for n=2:length
- X(:,n)=F*X(:,n-1);
- P=F*P*F'+ G*Q*G';
- Kg=P*H'/(H*P*H'+R);
- X(:,n)=X(:,n)+Kg*(Z(:,n)-H*X(:,n));
- P=(eye(9,9)-Kg*H)*P;
- end
- %% 繪圖代碼
- figure(1)
- plot(y(1,:))
- hold on;
- plot(y(2,:))
- hold off;
- title('理想的觀測(cè)量');
- figure(2)
- plot(Z(1,:))
- hold on;
- plot(Z(2,:))
- hold off;
- title('帶有噪聲的觀測(cè)量');
- figure(3)
- plot(X(1,:))
- hold on;
- plot(X(2,:))
- hold off;
- title('濾波后的觀測(cè)量');
|
評(píng)分
-
查看全部評(píng)分
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
