mainkalman.m

filter kalman with matlab

function [esp,esv,esa,enorm,mdetector]=mainkalman(nX,Qm,alpha,sigma,sampling_time,initout); 


resolution=size(nX,2);
a = alpha;                          % alpha
T = sampling_time;                  % sampling time
s = sigma;                          % sigma

Qp = processnoise(a,s,T);           % Compute process noise.

A = zeros(3,3);                     % Define dynamic matrix of system.
A(1,1) = 1;
A(1,2) = T;
A(1,3) = (a*T-1+exp(-a*T))/a^2;
A(2,1) = 0;
A(2,2) = 1;
A(2,3) = (1-exp(-a*T))/a;
A(3,1) = 0;
A(3,2) = 0;
A(3,3) = exp(-a*T);

H = [1 0 0];                        % Define measurement matrix.

x1=zeros(3,1);                      % x1 is predicted state.
x=initout*[1;0;0];                  % x is corrected state.
p1=zeros(3,3);                      % p1 is priori covariance matrix.
p=zeros(3,3);                       % p is posteriori covariance matrix.
k=zeros(3,1);                       % k is kalamn gain.
mdetector=zeros(1,resolution);

for n=1:resolution
    x1 = A*x;
    p1 = A*p*A'+Qp;
    S = H*p1*H' + Qm;
    k = (p1*H')/S;
    v = nX(n) - (H*x1);             % V is innovation coeficient.
    x = x1 + k*v;
    p = (eye(3) - k*H)*p1;
    esp(n)=x(1,1);
    esv(n)=x(2,1);
    esa(n)=x(3,1);
    e(n)=trace(p);
    %---Manoeuvre detection---
    enorm(n)=v'*S^-1*v;
    if n>1
        mdetector(n)=.95*mdetector(n-1)+enorm(n);
    end;
    %--------------------------
end;
return;