%% In off-line navigation applications (e.g. post mission trajectory analysis),
%% we may first prefer to process the raw imu data using only no-motion updates.
%% Essentiall, each no motion instant provides an oportunity to self calibrate
%% the imu itself. As smoothers are robust to modelling errors, pre-processing
%% the raw imu data with smoothers for no-motion updates help us to eliminate
%% unknown (and unmodelled) biases before the imu data are actually processed by the INS.
%%
%% However, when you use a smoother to generate such a self calibrated imu data,
%% you must also compute the parameters of the Markovian error model parameters
%% of the new (smoothed) data set. (These model parameters are required in navigation
%% filter/smooters).
%%
%% Computing the smoothing error model parameter can be insanely difficult for
%% the standard smoother implementation such as RTS, BF etc.
%% On the other hand, the intermadiate variables of Weinert's complementary space
%% approach directly corresponds to these smoothing error model parameters. Therefore,
%% weinert's smoothers are perfect for such imu pre-smoothing applications.
%%%%IMU Pre-Smoother
clear;
DIRNAME='C:\Documents and Settings\Yigiter\Desktop\My Dropbox\SRIMU\Smoother\BeniSil';
%For MC
% rstr=RandStream('mt19937ar','seed',8967);
% RandStream.setDefaultStream(rstr);
rstr=RandStream.getDefaultStream();
reset(rstr,83474);
rstat=rstr.State;
%%Sensor error definitions at 1Hz
%Accelerometer
SenErrDef(1).A=diag([0.9988 1]);
SenErrDef(1).B=diag([3.5e-005 0]);
SenErrDef(1).C=[1 1];
SenErrDef(1).D=diag([0.001]);
SenErrDef(1).sP=diag([0.0007 0.01]);
SenErrDef(1).tparam=[1e-3 5e-5]; %first param=sP of temperature scale factor (RC), second=B of temperature RW
SenErrDef(2).A=diag([0.99885 1]);
SenErrDef(2).B=diag([3.5e-006 0]);
SenErrDef(2).C=[1 1];
SenErrDef(2).D=diag([0.004]);
SenErrDef(2).sP=diag([0.0008 0.001]);
SenErrDef(2).tparam=[4e-3 1e-5];
SenErrDef(3).A=diag([0.9987 1]);
SenErrDef(3).B=diag([3.5e-004 0]);
SenErrDef(3).C=[1 1];
SenErrDef(3).D=diag([0.0001]);
SenErrDef(3).sP=diag([0.00007 0.01]);
SenErrDef(3).tparam=[1e-4 8e-5];
SenErrDef(4).A=diag([0.9989 1]);
SenErrDef(4).B=diag([3.5e-004 0]);
SenErrDef(4).C=[1 1];
SenErrDef(4).D=diag([0.006]);
SenErrDef(4).sP=diag([0.0003 0.06]);
SenErrDef(4).tparam=[1e-4 5e-4];
% SenErrDef(2)=SenErrDef(1);
% SenErrDef(3)=SenErrDef(1);
% SenErrDef(4)=SenErrDef(1);
%Sensor configuration matrix
M=[cos(pi/2) sin(pi/2);
cos(2*pi/3) sin(2*pi/3);
cos(pi/4) sin(2*pi/4);
cos(0) sin(0)];
%temperature model
tem_mod.A=1;
tem_mod.B=0.1;
tem_mod.u=0;
%%%Generate sensor outputs
[rstat]=AddIMUErr_v001([], [DIRNAME '\imu.bin'], [DIRNAME '\imuerr.bin'], 2000, zeros(4,1), SenErrDef, tem_mod, rstat);
%read/create i/o files
[nst nstTI nstTV]=st_size_v000(SenErrDef);
sendat=readbin_v000([DIRNAME '\imu.bin'],size(M,1)+2);
errdat=readbin_v000([DIRNAME '\imuerr.bin'],nst+1);
FSMO_BUF=fopen([DIRNAME '\smobuf.bin'],'wb');
FSMO_RES=fopen([DIRNAME '\smores.bin'],'wb');
FSMO_ERR=fopen([DIRNAME '\smoerr.bin'],'wb');
FSMO_COV=fopen([DIRNAME '\smocov.bin'],'wb');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%just for comparison (not needed)
FFIL_RES=fopen([DIRNAME '\filres.bin'],'wb');
FFIL_ERR=fopen([DIRNAME '\filerr.bin'],'wb');
FFIL_COV=fopen([DIRNAME '\filcov.bin'],'wb');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Sys model for TI part
[Ati, Bti, Cti, Dti, sPti]=imu_modTI_v000(SenErrDef);
%observation model (assumed to be TI for this example)
[T MLS]=cp_T_v000(M,Dti*Dti',0);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%% Backward iterations %%%%%%%%%%%%%%%%
z=zeros(nst,1); %backward best estimate=Pinvb*x
Pbinv=zeros(nst);
%Last obs.
y=T*sendat(3:end,end);
tem=sendat(2,end);
[Atv, Btv, Ctv]=imu_modTV_v000(SenErrDef, 0, tem, 0);
Csys=T*[Cti Ctv];
Dsys=T*Dti;
Rinv=inv(Dsys*Dsys');
z=Csys'*Rinv*y;
Pbinv=Csys'*Rinv*Csys;
%Iterate for the rest of the observations
for in=(size(sendat,2))-1:-1:1
tem=sendat(2,in);
tem_dif=sendat(2,in+1)-sendat(2,in);
[Atv, Btv, Ctv]=imu_modTV_v000(SenErrDef, tem_dif, tem, 0);
if (sendat(1,in)>=1000 && sendat(1,in)<=1200) %Additional IMU observation (in this case, direct observation from stationarity)
Msys=[M;1 0;0 1];
Robs=diagmat_v000(zeros(2),Dti*Dti',[]);
[Tsys MLSsys]=cp_T_v000(Msys,Robs,1);
yobs=[sendat(3:end,in);zeros(2,1)];
Cobs=[Cti Ctv;zeros(2,nst)];
else
Tsys=T;
MLSsys=MLS;
Robs=Dti*Dti';
yobs=sendat(3:end,in);
Cobs=[Cti Ctv];
end
y=Tsys*yobs;
Asys=[Ati zeros(nstTI,nstTV);zeros(nstTV,nstTI) Atv];
Bsys=[Bti zeros(nstTI,nstTV);zeros(nstTV,nstTI) Btv];
Csys=Tsys*Cobs;
Rsys=Tsys*Robs*Tsys';
smo_mat=inv(eye(nst)+Pbinv*Bsys*Bsys');
Rinv=inv(Rsys);
%%write to the buffer
fwrite(FSMO_BUF,[z;smo_mat(:)],'double');
%compute the new values
z=Asys*smo_mat*z+Csys'*Rinv*y;
Pbinv=Asys'*smo_mat*Pbinv*Asys+Csys'*Rinv*Csys;
end
fclose(FSMO_BUF);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%% Forward iterations %%%%%%%%%%%%%%%%%%%
%First data (iterations performed on best estimates)
tem=sendat(2,1);
tem_dif=sendat(2,2)-sendat(2,1);
[Atv, Btv, Ctv, sPtv]=imu_modTV_v000(SenErrDef, tem_dif, tem, 1);
Psys=diagmat_v000(sPtv*sPtv',sPti*sPti',[]);
Asys=diagmat_v000(Atv,Ati,[]);
Bsys=diagmat_v000(Btv,Bti,[]);
Qsys=Bsys*Bsys';
Csys=T*[Cti Ctv];
Dsys=T*Dti;
Rsys=Dsys*Dsys';
Rinp=MLS*(Dti*Dti')*MLS';
Cinp=MLS*[Cti Ctv];
%Psmo=inv((inv(Psys)+Pbinv));
Psmo=inv(eye(nst)+Psys*Pbinv)*Psys;
xsmo=Psmo*z;
usmo=MLS*sendat(3:end,1)-Cinp*xsmo;
FSMO_BUF=fopen([DIRNAME '\smobuf.bin'],'rb');
sz_smo=nst*(nst+1);
fseek(FSMO_BUF, -sz_smo*8,'eof');
%err model
vr_a=fread(FSMO_BUF,sz_smo,'double');
z=vr_a(1:nst);
smo_mat=reshape(vr_a(nst+1:end),nst,nst);
Aerr_smo=smo_mat'*Asys;
Qerr_smo=Qsys*smo_mat;
fwrite(FSMO_RES,[xsmo;usmo],'double');
fwrite(FSMO_ERR,[Aerr_smo(:);mat2vec_v000(Qerr_smo);Cinp(:);mat2vec_v000(Rinp)],'double');
fwrite(FSMO_COV,[mat2vec_v000(Cinp*Psmo*Cinp');mat2vec_v000(Psmo)],'double');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%filtered states (not required; just for comparison)
y=T*sendat(3:end,1);
K=Psys*Csys'*inv(Csys*Psys*Csys'+Rsys);
xfilt=K*y;
ufilt=MLS*sendat(3:end,1)-Cinp*xfilt;
Pfilt=(eye(nst)-K*Csys)*Psys;
Aerr_filt=(Asys-K*Csys*Asys);
Qerr_filt=Qsys+K*Rsys*K';
fwrite(FFIL_RES,[xfilt;ufilt],'double');
fwrite(FFIL_ERR,[Aerr_filt(:);mat2vec_v000(Qerr_filt);Cinp(:);mat2vec_v000(Rinp)],'double');
fwrite(FFIL_COV,[mat2vec_v000(Cinp*Pfilt*Cinp');mat2vec_v000(Pfilt)],'double');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%iterate for the rest
for (in=2:size(sendat,2)-1)
%smoothed value
xsmo=smo_mat'*Asys*xsmo+smo_mat'*Qsys*z;
Psmo=Aerr_smo*Psmo*Aerr_smo'+Qerr_smo;
%%System model at time "in"
tem=sendat(2,in);
tem_dif=sendat(2,in+1)-sendat(2,in);
[Atv, Btv, Ctv]=imu_modTV_v000(SenErrDef, tem_dif, tem, 0);
if (sendat(1,in)>=1000 && sendat(1,in)<=1200)
Msys=[M;1 0;0 1];
Robs=diagmat_v000(zeros(2),Dti*Dti',[]);
[Tsys MLSsys]=cp_T_v000(Msys,Robs,1);
yobs=[sendat(3:end,in);zeros(2,1)];
Cobs=[Cti Ctv;zeros(2,nst)];
else
Robs=Dti*Dti';
Tsys=T;
MLSsys=MLS;
yobs=sendat(3:end,in);
Cobs=[Cti Ctv];
end
Asys=diagmat_v000(Atv,Ati,[]);
Bsys=diagmat_v000(Btv,Bti,[]);
Qsys=Bsys*Bsys';
Csys=Tsys*Cobs;
Rsys=Tsys*Robs*Tsys';
Rinp=MLSsys*Robs*MLSsys';
Cinp=MLSsys*Cobs;
usmo=MLSsys*yobs-Cinp*xsmo;
%error model param
fseek(FSMO_BUF, -sz_smo*8*2,'cof'); %relocate the file pointer for the next read
vr_a=fread(FSMO_BUF,sz_smo,'double');
z=vr_a(1:nst);
smo_mat=reshape(vr_a(nst+1:end),nst,nst);
Aerr_smo=smo_mat'*Asys;
Qerr_smo=Qsys*smo_mat;
fwrite(FSMO_RES,[xsmo;usmo],'double');
fwrite(FSMO_ERR,[Aerr_smo(:);mat2vec_v000(Qerr_smo);Cinp(:);mat2vec_v000(Rinp)],'double');
fwrite(FSMO_COV,[mat2vec_v000(Cinp*Psmo*Cinp');mat2vec_v000(Psmo)],'double');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%filtered solution (just for comparison)
y=Tsys*yobs;
K=Pfilt*Csys'*inv(Csys*Pfilt*Csys'+Rsys);
Pfilt=(eye(nst)-K*Csys)*Pfilt;
xfilt=xfilt+K*(y-Csys*xfilt);
ufilt=MLSsys*yobs-Cinp*xfilt;
Aerr_filt=(Asys-K*Csys*Asys);
Qerr_filt=Qsys+K*Rsys*K';
fwrite(FFIL_RES,[xfilt;ufilt],'double');
fwrite(FFIL_ERR,[Aerr_filt(:);mat2vec_v000(Qerr_filt);Cinp(:);mat2vec_v000(Rinp)],'double');
fwrite(FFIL_COV,[mat2vec_v000(Cinp*Pfilt*Cinp');mat2vec_v000(Pfilt)],'double');
Pfilt=Asys*Pfilt*Asys'+Qsys;
xfilt=Asys*xfilt;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
%last data
tem=sendat(2,end);
tem_dif=0;
[Atv, Btv, Ctv]=imu_modTV_v000(SenErrDef, tem_dif, tem, 0);
Rinp=MLS*(Dti*Dti')*MLS';
Cinp=MLS*[Cti Ctv];
xsmo=smo_mat'*Asys*xsmo+smo_mat'*Qsys*z;
Psmo=Aerr_smo*Psmo*Aerr_smo'+Qerr_smo;
usmo=MLS*sendat(3:end,in)-Cinp*xsmo;
fwrite(FSMO_RES,[xsmo;usmo],'double');
fwrite(FSMO_ERR,[zeros(nst^2,1);mat2vec_v000(zeros(nst));Cinp(:);mat2vec_v000(Rinp)],'double');
fwrite(FSMO_COV,[mat2vec_v000(Cinp*Psmo*Cinp');mat2vec_v000(Psmo)],'double');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%filtered solution (just for comparison)
Csys=T*[Cti Ctv];
Rsys=(T*Dti)*(T*Dti)';
y=T*sendat(3:end,end);
K=Pfilt*Csys'*inv(Csys*Pfilt*Csys'+Rsys);
Pfilt=(eye(nst)-K*Csys)*Pfilt;
xfilt=xfilt+K*(y-Csys*xfilt);
ufilt=MLS*sendat(3:end,in)-Cinp*xfilt;
fwrite(FFIL_RES,[xfilt;ufilt],'double');
fwrite(FFIL_ERR,[zeros(nst^2,1);mat2vec_v000(zeros(nst));Cinp(:);mat2vec_v000(Rinp)],'double');
fwrite(FFIL_COV,[mat2vec_v000(Cinp*Pfilt*Cinp');mat2vec_v000(Pfilt)],'double');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fclose all;
% return;
%%%%% plot results
sendat=readbin_v000([DIRNAME '\imu.bin'],6);
mlsdat=MLS*sendat(3:6,:);
smodat=readbin_v000([DIRNAME '\smores.bin'],18);
fildat=readbin_v000([DIRNAME '\filres.bin'],18);
smocov=readbin_v000([DIRNAME '\smocov.bin'],nst*(nst+1)/2+3);
filcov=readbin_v000([DIRNAME '\filcov.bin'],nst*(nst+1)/2+3);
figure(1);
plot(mlsdat(1,:),'k')
hold on
plot(fildat(17,:))
plot(smodat(17,:),'r')
plot(filcov(1,:).^0.5,'--')
plot(smocov(1,:).^0.5,'r--')
figure(2);
plot(mlsdat(2,:),'k')
hold on
plot(fildat(18,:))
plot(smodat(18,:),'r')
plot(filcov(2,:).^0.5,'--')
plot(smocov(2,:).^0.5,'r--')