s_gps_ins_position_sp_demo.m

matlab 编写的gps/sins组合导航程序,附程序说明与数据输入文件

%GPS/INS组合导航

%%%%%%%%%%%%%%%%%%
%edit by horsejun
%%%%%%%%%%%%%%%%%%

%量测信号：   位置
%INS输出数据由simulink计算得出
%参考文献《GPS导航原理与应用》——王惠南

clear
clc

%得到轨迹信号

load ode500

Re 		= 6378245;   																						  %地球长半径

%真实轨迹
a_R 	= yout(:,1:3);
v_R	  = yout(:,4:6);
p_R 	= yout(:,7:9);

%加噪声后的INS计算结果
a_ins = yout(:,10:12);
v_ins = yout(:,13:15);
p_ins = yout(:,16:18);
quat  = yout(:,19:22);         																	%姿态四元数
Fn    = yout(:,23:25);                            							%地理系下的比力

%惯导相关的噪声统计数据
Q_wg  = (0.04/(57*3600))^2;                      							  %陀螺马氏过程
Q_wr  = (0.01/(57*3600))^2;                       							%陀螺白噪声
Q_wa  = (1e-3)^2;   %加计马氏过程
Q 		= diag([Q_wg Q_wg Q_wg,  Q_wr Q_wr Q_wr,  Q_wa Q_wa Q_wa]);
Tg 		= 300*ones(3,1);
Ta 		= 1000*ones(3,1);

%得到带误差的GPS输出信号
p_gps_sample 		= p_R(1:10:end,:);
n 							= size(p_gps_sample,1);
p_error(:,1:2)  = 30*randn(n,2)/Re;    
p_error(:,3) 		= 30*randn(n,1);                                %位置误差
p_gps 					= p_gps_sample+p_error;                         %加入位置误差
R 							= diag(std(p_error).^2);                        %计算测量噪声方差R

%卡尔曼滤波
tao 						= 1;                                            %滤波步长
a_ins_sample 		= a_ins(1:10:end,:);
v_ins_sample 		= v_ins(1:10:end,:);
p_ins_sample 		= p_ins(1:10:end,:);
a_R_sample 			= a_R(1:10:end,:);
v_R_sample 			= v_R(1:10:end,:);
p_R_sample 			= p_R(1:10:end,:);
Dp 							= p_ins_sample-p_gps;                           %INS与GPS输出的位置差值
a 							= a_ins_sample; 
v 						  = v_ins_sample;
p 						  = p_ins_sample;
quat0 					= quat(1:10:end,:);
Fn0 						= Fn(1:10:end,:);

[Error_a, Error_v, Error_p, PP] = kalman_GPS_INS_position_sp_NFb(Dp, v, p, quat0, Fn0, Q, R, Tg, Ta, tao);   %得到位置，速度误差误差估计值

a_estimate 			= a(1:size(Error_a,1),:)-Error_a;
v_estimate 			= v(1:size(Error_v,1),:)-Error_v;
p_estimate 			= p(1:size(Error_p,1),:)-Error_p;

n = size(p_estimate,1);

%位置误差比较
figure  
subplot(3,1,1)
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')  %黑线-滤波前的误差   红线-滤波后的误差
xlabel('时间，单位s')
subplot(3,1,2)
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')  %黑线-滤波前的误差   红线-滤波后的误差
ylabel('位置误差，单位m')
subplot(3,1,3)
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')  						%黑线-滤波前的误差   红线-滤波后的误差
xlabel('黑线-滤波前的INS误差   红线-滤波后的误差')

%速度误差比较
figure  
subplot(3,1,1)
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') 						  %黑线-滤波前的误差   红线-滤波后的误差
xlabel('时间，单位s')
subplot(3,1,2)
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')  						%黑线-滤波前的误差   红线-滤波后的误差
ylabel('速度误差，单位m/s')
subplot(3,1,3)
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')  						%黑线-滤波前的误差   红线-滤波后的误差
xlabel('黑线-滤波前的INS误差   红线-滤波后的误差')

%位置误差
figure  
subplot(3,1,1)
xlabel('时间，单位s')
plot((1:n),(p_R_sample(1:n,1)-p_estimate(:,1))*6370000,'r')  																				%红线-滤波后的误差
subplot(3,1,2)
plot((1:n),(p_R_sample(1:n,2)-p_estimate(:,2))*6370000,'r')  																				%红线-滤波后的误差
ylabel('位置误差，单位m')
subplot(3,1,3)
plot((1:n),p_R_sample(1:n,3)-p_estimate(:,3),'r')  																								  %红线-滤波后的误差
xlabel('滤波后的位置误差')

%速度误差
figure  
subplot(3,1,1)
plot((1:n),v_R_sample(1:n,1)-v_estimate(:,1),'r')  																									% 红线-滤波后的误差
xlabel('时间，单位s')
subplot(3,1,2)
plot((1:n),v_R_sample(1:n,2)-v_estimate(:,2),'r')  																									%红线-滤波后的误差
ylabel('速度误差，单位m/s')
subplot(3,1,3)
plot((1:n),v_R_sample(1:n,3)-v_estimate(:,3),'r')  																									% 红线-滤波后的误差
xlabel('滤波后的速度误差')