root/library/tests/testKF_big.m @ 550

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

if ~skipgen
        dimx = 30;
        Apom=rand(dimx);
        Apom2 = Apom*Apom';
        A = Apom2/max(1.1*eig(Apom2));
        B=ones(dimx,1);
        C=eye(dimx);
        D=ones(dimx,1);
        R=0.1*eye(dimx);
        Q=0.2*eye(dimx); 

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

        N =1000;
        mu0 = zeros(dimx,1);
        P0 = 200*eye(dimx);

        u = rand(1,N);
        x = zeros(dimx,N);
        y = zeros(dimx,N);

        x(:,1) = 10*ones(dimx,1);
        Et = sQ*randn(dimx,N);
        Wt = sR*randn(dimx,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('testKF.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;
Oxt2 = mu0;
OPt2 = P0;
ll2=0;

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:dimx,t)=mu;

%       [Oxt,OPt,ll(t)] = Kalman(Oxt,y(:,t),A,C,Q,R,OPt);
%       [Oxt2,OPt2,ll2(t)] = Kalman(Oxt2,y(:,t),A,C,Q,R/10000,OPt2);
end
exec_matlab = toc

!cd ../;./tests/testKF
itload('testKF_res.it');

hold off
plot(x');
hold on
plot(xth','--');
plot(xth2','+');
plot(xthE','o');
plot(Mu','d');

max(max(x(:,2:end) - xth(:,1:end-1)))
max(max(x - xth2))
max(max(x - xthE))
max(max(x - Mu))

exec_times
exec_matlab./exec_times
keyboard
end