root/matlab/testKF.m @ 8

if 0
A=[1 -0.5; 1 0];
B=[1;0];
C=[1 0];
D=[0];
R=[0.1];
Q=[0.2 0 ; 0 0.02]; 

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

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

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

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

d=[y;u];
itsave('testKF.it',d,A,B,C,D,Q,R,P0,mu0)
save testKF
else
%!cd ../testKF

load testKF
% init 
mu = mu0;
P = P0;
EP = [0;0];

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;
        EP(:,t) = eig(P); 
        if any(EP<0), keyboard; end
        mu = mu + K*(y(t)-C*mu-D*u(t));
        Mu(1:2,t)=mu;
end



itload('testKF_res.it');

hold off
plot(x');
hold on
plot(xth','--');
plot(xth2','+');
plot(Mu',':');
end