AddIMUErr_v002

% A better version of AddIMUErr_v001 (I will depreciate the _v001 version.)

% This script assumes that the Output of each sensor is modelled as:
%   x_dot=A*x+Bw
%   y=u+Cti*x+((M*u_all)'.*Ctv)*x+Dv
%   where,
%       x is the error states
%       E[|Bw|^2]=Q,
%       y is the sensor output (this is a scalar)
%       u is the input (true acceleration/rotation rate)
%       u_all=true_inp(:,in) all true input values
%       E[|Dv|^2]=R
%       The deafult value of M*u_all is u, where u_all is the entire input (true_inp) vector.

% This script ignores squaring type of errors (2nd order effects). Perhaps, I may add these effects
%  in a future version.
% This script also is not capable of simulating temperature dependent errors. (_v001 version was
%  capable of doing that.)

% ErrDefs is an array of cell. Each cell defines the error model of the corresponding row of true_inp.
% Each member of ErrDefs must be as follows:
%   ErrDefs{i}.A (If this is empty or not defined, only white noise will be added according to R matrix.)
%   ErrDefs{i}.Q, where B*B'=Q
%   ErrDefs{i}.Cti
%   ErrDefs{i}.Ctv  (This can be undefined or empty if there is no scale factor or cross axis errors)
%   ErrDefs{i}.P0, where P0=E{|x0|^2}
%   ErrDefs{i}.R, where D*D'=R
%   ErrDefs{i}.M (M can be undefined or empty. Unless you use SRIMU, completely ignore M.)
%The number of struct and number of rows of true_inp MUST be the same.

% dt is the sampling period of true_inp. If it does not exist or empty, we 
% assume that ErrDefs are defined in discrete time

function [sensor_out, sensor_err]= AddIMUErr(true_inp, ErrDefs, dt)

%Check the arguments' integrity
[nsen, ndat]=size(true_inp); %Each row corresponds a different sensor
if (nsen~=length(ErrDefs))
    disp('The number of cells and number of rows of true_inp must be the same');
    return;
end

%Generate M matrices unless they are defined or not empty.
%The default M matrix corresponds to scale factor error (i.e. u)
scale_err=zeros(nsen,1);
for in=1:nsen
    if isfield(ErrDefs{in}, 'Ctv') && ~isempty(ErrDefs{in}.Ctv)
        scale_err(in)=1;
        %Define the default value of M
        if (~isfield(ErrDefs{in}, 'M')) || isempty(ErrDefs{in}.M)
            ErrDefs{in}.M=zeros(size(ErrDefs{in}.Ctv,2),size(true_inp,1));
            ErrDefs{in}.M(:,in)=1;
        end
    else
        scale_err(in)=0;
    end
end

%Determine if we need an error state for the correponding sensor. If we don't,
%simply use 0 matrices for the sake of less code.
for in=1:nsen
    if ~isfield(ErrDefs{in}, 'A') || isempty(ErrDefs{in}.A)
        ErrDefs{in}.A=0;
        ErrDefs{in}.Q=0;
        ErrDefs{in}.Cti=0;
        ErrDefs{in}.Ctv=0;
        ErrDefs{in}.P0=0;
    end
end


%Convert to discrete time
if (nargin==3)  %the model is in continuous time. Convert it to discrete time
    for in=1:nsen
        [A, Q, R]=dc2dc_v000(ErrDefs{in}.A, ErrDefs{in}.Q, ErrDefs{in}.R,dt,0,[]);
        ErrDefs{in}.A=A;
        ErrDefs{in}.Q=Q;
        ErrDefs{in}.R=R;
    end
end

%Convert covariances into coefficients and define M matrices
nsta=0;
for in=1:nsen
    %Coefficients for IMU error state generation
    [L,D]=ldl(ErrDefs{in}.Q);
    ErrDefs{in}.B=L*D.^0.5;
    [L,D]=ldl(ErrDefs{in}.R);
    ErrDefs{in}.D=L*D.^0.5;
    [L,D]=ldl(ErrDefs{in}.P0);
    ErrDefs{in}.STD0=L*D.^0.5;

    %total number of states
    nsta=nsta+size(ErrDefs{in}.A,1);
end

%Output variables
sensor_out=zeros(nsen,ndat);
sensor_err=zeros(nsta,ndat);

%Generate and add errors
%.%Combine all models into a single matrix
A=[];
B=[];
Cti=[];
Ctv_scaled=[];
STD0=[];
D=[];
for in=1:nsen
    A=diagmat_v000(ErrDefs{in}.A,A,1);
    B=diagmat_v000(ErrDefs{in}.B,B,1);
    Cti=diagmat_v000(ErrDefs{in}.Cti,Cti,1);
    if (scale_err(in))
        Ctv_scaled=diagmat_v000((ErrDefs{in}.M*true_inp(:,1))'.*(ErrDefs{in}.Ctv),Ctv_scaled,1);
    else
        Ctv_scaled=diagmat_v000(zeros(1,size(ErrDefs{in}.A,1)),Ctv_scaled,1);
    end
    STD0=diagmat_v000(ErrDefs{in}.STD0,STD0,1);
        
    D=diagmat_v000(ErrDefs{in}.D,D,1);
end


sensor_err(:,1)=STD0*randn(nsta,1);
sensor_out(:,1)=true_inp(:,1)+(Cti+Ctv_scaled)*sensor_err(:,1)+D*randn(nsen,1);
    

%.%Generate the rest
for in=2:ndat
    %Time varying component
    if (scale_err)
        Ctv_scaled=[];
        for k=1:nsen
            Ctv_scaled=diagmat_v000((ErrDefs{k}.M*true_inp(:,in))'.*(ErrDefs{k}.Ctv),Ctv_scaled,1);
        end
    end
    
    %Generate and add errors
    sensor_err(:,in)=A*sensor_err(:,in-1)+B*randn(nsta,1);
    sensor_out(:,in)=true_inp(:,in)+(Cti+Ctv_scaled)*sensor_err(:,in)+D*randn(nsen,1);
end