卡爾曼濾波器在INS-GPS組合導航中應用仿真 matlab源代碼

ID:989853 · 發(fā)表于 2021-12-9 09:04

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

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

運行結果：

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 卡爾曼濾波器在INS-GPS組合導航中應用仿真
% Author : lylogn
% Email : lylogn@gmail.com
% Company: BUAA-Dep3
% Time : 2013.01.06
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 參考文獻:
% [1]. 鄧正隆. 慣導技術, 哈爾濱工業(yè)大學出版社.2006.
clear all;
%% 慣性-GPS組合導航模型參數(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; %這三個參數(shù)的含義詳見參考文獻
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)化為離散時間系統(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;];%觀測矩陣
%% 卡爾曼濾波器參數(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); %測量噪聲協(xié)方差
y(1,:)=2*sin(pi*t*0.5);
y(2,:)=2*cos(pi*t*0.5);
Z=y+sqrt(R)*randn(2,length); %生成的含有噪聲的假定觀測值，2維
X=zeros(9,length); %狀態(tài)估計值，9維
X(:,1)=[0,0,0,0,0,0,0,0,0]'; %狀態(tài)估計初始值設定
P=eye(9); %狀態(tài)估計協(xié)方差
%% 卡爾曼濾波算法迭代過程
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('理想的觀測量');
figure(2)
plot(Z(1,:))
hold on;
plot(Z(2,:))
hold off;
title('帶有噪聲的觀測量');
figure(3)
plot(X(1,:))
hold on;
plot(X(2,:))
hold off;
title('濾波后的觀測量');

復制代碼

帳號		自動登錄	找回密碼
密碼			立即注冊

卡爾曼濾波器在INS-GPS組合導航中應用仿真 matlab源代碼

評分

瀏覽過的版塊