clear;
fclose all;
randn('state',[112233;445566]); %make non-random (required for debugging)
PATH='C:\Work\Benisil';
%%Specify the imu error model and generate the path
[SenErrDef, IniErrDef, ObsErrDef, ini_pva, dt]=sys_path(PATH);
nst=15;
%%Open the files
%Filter
F_IMU1=fopen([PATH '\imu.bin'],'rb');
F_TRU1=fopen([PATH '\mnav.bin'],'rb');
F_IMUERR1=fopen([PATH '\imuerr.bin'],'rb');
F_OBS_POS1=fopen([PATH '\obs_pos.bin'],'rb');
F_RES1=fopen([PATH '\res1.bin'],'wb');
%smoother
F_PROP=fopen([PATH '\smo_prop.bin'],'wb');
F_UPDT=fopen([PATH '\smo_updt.bin'],'wb');
%%Program parameters
prp_per=round(0.33/dt); %propagation time for the discretized system
%%Sensor error models
[dAimu, dBimu, Cimu, dDimu, sP0imu]=imu_modTI_v000(SenErrDef); %errors models in discrete time
[Aimu, Qimu, Rimu]=dc2dc_v000(dAimu, dBimu*dBimu', dDimu*dDimu', dt, 1, []); %models in continious time
%%%%%%%%Rimu=Rimu/dt; %%correction for the notation difference. (we need a standard notation for mems errors!!!!!)
%Initial covariance
P1=zeros(nst);
P1(1:3,1:3)=IniErrDef.pos_sP*IniErrDef.pos_sP';
P1(4:6,4:6)=IniErrDef.vel_sP*IniErrDef.vel_sP';
P1(7:9,7:9)=IniErrDef.att_sP*IniErrDef.att_sP';
P1(10:nst,10:nst)=sP0imu*sP0imu';
%imu error states
ximu1=zeros(6,1);
%INS registers
pos_n1=ini_pva(:,1);
att_n1=ini_pva(:,3);
Cbn1=euler2dcm_v000(att_n1);
vel_n1=Cbn1*ini_pva(:,2); %note path generator is defined for body frame velocity
%accumulators and control flags
avg_imu1=zeros(6,1);
pr_coun1=0;prp_coun1=0;
STM_upd=eye(nst);
write_flag1=0;
I=eye(nst);
%%read the input data from the files
imu_data1=fread(F_IMU1,8,'double'); %garbage
imu_data1=fread(F_IMU1,8,'double');
true_val1 = fread(F_TRU1, 10, 'double');
imu_err1=fread(F_IMUERR1, 7, 'double');
obs_pos1=fread(F_OBS_POS1, 4, 'double');
%%Start iterations
while (~feof(F_IMU1))
imu1=imu_data1(3:8)-ximu1;
[Cbn1, vel_n1, pos_n1]=strapdown_ned_dcm_v000(Cbn1, vel_n1, pos_n1, imu1(1:3), imu1(4:6), dt);
avg_imu1=avg_imu1+imu1;
pr_coun1=pr_coun1+1;
prp_coun1=prp_coun1+1;
if (mod(prp_coun1,prp_per)==0 && prp_coun1~=0) %propagate the covariance and estimates
avg_imu1=avg_imu1/prp_coun1;
%%Form the discrete time navigation model
[STM1 Q1]=mdl_ned_dcm_v000(pos_n1, vel_n1, Cbn1, avg_imu1(1:3), Aimu, Qimu, Cimu, Rimu, prp_coun1*dt);
%Propagate
P1=STM1*P1*STM1'+Q1;
STM_upd=STM1*STM_upd;
prp_coun1=0;
avg_imu1=zeros(6,1);
%update IMU error states
ximu1=STM1(10:nst,10:nst)*ximu1;
write_flag1=1;
%smoother stuff;
smo_STM=STM_upd(:);
STM_upd=eye(nst);
end
if (~isempty(obs_pos1) && pr_coun1==obs_pos1(1))
%first check wheter the covariance is uptodate
if (prp_coun1~=0)
avg_imu1=avg_imu1/prp_coun1;
[STM1 Q1]=mdl_ned_dcm_v000(pos_n1, vel_n1, Cbn1, avg_imu1(1:3), Aimu, Qimu, Cimu, Rimu, prp_coun1*dt);
P1=STM1*P1*STM1'+Q1;
STM_upd=STM1*STM_upd;
prp_coun1=0;
avg_imu1=zeros(6,1);
ximu1=STM1(10:nst,10:nst)*ximu1;
%smoother stuff;
smo_STM=STM_upd(:);
STM_upd=eye(15);
end
smo_dx_inp=0;
smo_dP_inp=0;
while (~isempty(obs_pos1) && (pr_coun1==obs_pos1(1))) %there may be more than one observation at any instant
H1=zeros(3,nst); %position update
H1(:,1:3)=eye(3);
R1=ObsErrDef(1).sR*ObsErrDef(1).sR';
inno1=pos_n1-obs_pos1(2:4);
Rinno_inv1=inv(H1*P1*H1'+R1);
K1=P1*H1'*Rinno_inv1;
%smoother stuff
smo_dx_inp=smo_dx_inp+STM_upd'*H1'*Rinno_inv1*inno1(:);
smo_dP_inp=smo_dP_inp+STM_upd'*H1'*Rinno_inv1*H1*STM_upd;
%correct current states
dx1=K1*inno1;
pos_n1=pos_n1-dx1(1:3);
vel_n1=vel_n1-dx1(4:6);
mx_a=euler2dcm_v000(dx1(7:9));
Cbn1=mx_a*Cbn1;
ximu1=ximu1+dx1(10:nst);
%update cov
P1=(I-K1*H1)*P1*(I-K1*H1)'+K1*R1*K1';
%prepare for the next cycle
STM_upd=(I-K1*H1)*STM_upd;
obs_pos1=fread(F_OBS_POS1, 4, 'double');
write_flag1=1;
end
%Smoother stuff
mx_a=smo_dP_inp(:); %half...
mx_b=STM_upd(:);
fwrite(F_UPDT,[pr_coun1;smo_dx_inp;mx_a;mx_b],'double');
STM_upd=eye(15);
end
%True values
true_val1 = fread(F_TRU1, 10, 'double');
imu_err1=fread(F_IMUERR1, 7, 'double');
%%%write the results
%read the true values
if (write_flag1)
err1=zeros(nst,1);
err1(1:3)=pos_n1-true_val1(2:4);
Cbn_true=euler2dcm_v000(true_val1(8:10));
err1(4:6)=vel_n1-Cbn_true*true_val1(5:7);
err1(7:9)=-dcm2euler_v000(Cbn1*Cbn_true');
err1(10:nst)=imu_err1(2:7)-ximu1;
fwrite(F_RES1,[pr_coun1;err1;diag(P1).^0.5],'double');
write_flag1=0;
%smoother stuff
smo_p=P1(:);
fwrite(F_PROP,[pr_coun1;smo_p;smo_STM],'double');
end
%read the next imu output
imu_data1=fread(F_IMU1,8,'double');
end
fclose all;
res1=readbin_v000([PATH '\res1.bin'],31);
figure;
plot(res1(1,:)/100,abs(res1(2,:))*6e6)
hold on
plot(res1(1,:)/100,(res1(17,:))*6e6,'--');
title('lat. error');
ylabel('m');
xlabel('sec');
legend('filter error', 'filter std');
grid;
return;
figure;
yy=6;
plot(res1(1,:)/100,abs(res1(1+yy,:)))
hold on
plot(res1(1,:)/100,(res1(16+yy,:)),'--');
xlabel('sec');
legend('filter', 'fls');
grid;
figure;
yy=2;
plot(res1(1,:)/100,abs(res1(1+yy,:))*6e6)
hold on
plot(res1(1,:)/100,(res1(16+yy,:))*6e6,'--');
xlabel('sec');
legend('filter', 'fls');
grid;