root/library/tests/stresssuite/kalman_stress.m @ 1353

function testKF(skipgen)
if nargin<1, skipgen=0; end

if ~skipgen
        A=[1 -0.5; 1 0];
        B=[1;0.1];
        C=[1 0];%; 0 1];
        D=0.1;%[0.1; 0];
        R=0.01;%[1 0; 0 0.1];
        Q=[0.2 0 ; 0 0.2]; 

        sQ = chol(Q)';
        sR = chol(R)';

        N =3000;
        mu0 = [0;0];
        P0 = 200*eye(2);

        u = zeros(1,N);
        x = zeros(2,N);
        y = zeros(1,N);

        x(:,1) = [10;10];
        Et = sQ*randn(2,N);
        Wt = sR*randn(1,N);
        for i=2:N;
                x(:,i) = A*x(:,i-1) + B*u(i) + Et(:,i);
                y(:,i) = C*x(:,i) + D*u(i) + Wt(:,1);
        end

        d=[y;u];
        itsave('kalman_stress.it',d,A,B,C,D,Q,R,P0,mu0)
        save testKF
else
        load testKF
end
% init 
mu = mu0;
P = P0;
EP = [0;0];

Oxt = mu0;
OPt = P0;
ll =0;
oxt = mu0;
oPt = chol(P0)';
oll=0;

Mu = zeros(2,N);
Mu_oo = zeros(2,N);

tic;
for t=2:N
        mu = A*mu + B*u(t);
        P  = A*P*A' + Q;

        %Data update
        Ry = C*P*C' + R;
        iRy = inv(Ry);
        K = P*C'*iRy; 
        P = P- K*C*P; % P = P -KCP;
        mu = mu + K*(y(:,t)-C*mu-D*u(t));
        Mu(1:2,t)=mu;

%       [Oxt,OPt,ll(t)] = Kalman(Oxt,y(:,t),A,C,Q,R,OPt);
%       [oxt,oPt,oll(t)] = KalmanSq(oxt,y(:,t),A,C,sQ,sR,oPt);
%       MuK(1:2,t) = Oxt;
%       MuS(1:2,t) = oxt;
end
exec_matlab = toc
%keyboard

%%%%%%%% OBJECTs in MATLAB %%%%%
addpath ../../../applications/bdmtoolbox/mex/mex_classes
addpath ../../../applications/bdmtoolbox/mex/

oKAL=mexKalman;
oKAL.A = A;
oKAL.B = B;
oKAL.C = C;
oKAL.D = D;
oKAL.Q = Q;
oKAL.R = R;
oKAL=oKAL.validate;
oKAL.apost_pdf.mu = mu0;
oKAL.apost_pdf.R = P0;

tic;
for t=2:N
    oKAL=oKAL.bayes(y(t),u(t));
    Mu_oo(1:2,t) = oKAL.apost_pdf.mu;
end
exec_matlab_oo=toc

!./stresssuite kalman_stress
itload('kalman_stress_res.it');

hold off
plot(x');
hold on
plot([Mu]','--'); % shift the predldmatictions
plot([Mu_oo]',':'); % shift the predldmatictions
plot(xth2','+');
plot(xthE','o');
%plot([zeros(size(xth,1),1) MuK]','d'); % shift the predictions

exec_times
exec_matlab./exec_times
%keyboard
end