%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('濾波后的速度誤差')