% Kalman Filter example for CS475 class

% this is a simple example of a robot moving in 2D. It has an inaccurate
% initial estimate of its position, but is able to measure (with noise) its
% x-y coordinate and thus update its position (using Kalman Filter).
%
% Black dot is the true (unknown-to-robot) position, red cross is its
% estimated position. The blue ellipse signifies uncertainty in each
% direction (one standard deviation away from the estimate)
%
% The model as is uses a perfectly known control input (U) to move the robot
% around, and thus the robot knows perfectly its location change in each
% timestep. Such use of control input is not usual, and the position change
% would be much better put into the transition step, by including velocity
% in the state description

% model specification:
DeltaT=0.2;     % timestep size
F=[1 0; 0 1];   % transition matrix
B=[1; 1];       % control matrix
Q=[1 0; 0 1];   % transition noise covariance
H=[1 0; 0 1];   % sensor matrix
R=[1 0; 0 1];   % sensor noise covariance
Theta=-pi/8;    % anlge of the movement of the robot

U=[cos(Theta); sin(Theta)].*[5; 5].*DeltaT;    % control input (position change at each timestep)


% initial robot position
X0=[1; -3];     % initial state
X0bar=[4; -2];  % initial estimate
P0=[1 0; 0 1];  % initial state uncertainty

% set the plotting environmet
hold off
plot(0,0)
axis([-1 10 -8 1]);
axis equal
line([0 10],[0 0],'LineWidth',4)
line([0 0],[0 -8],'LineWidth',4)
hold on

% run the simulation
X=X0;           % current state (unknown to the robot)
Xbar=X0bar;     % current state estimate (known to the robot)
P=P0;           % current state uncertainty (known to the robot)

for T=0:DeltaT:1.6

    % draw robot and elipsoid
    plot(X(1),X(2),'.k','MarkerSize',20)
    plot(Xbar(1),Xbar(2),'+r')
    [xs, ys]=sigmaEllipse(P, Xbar);
    plot(xs,ys)

    % update the true state: 
    X=F*X+[1;1].*U;

    % make observation:
    Z=H*X;

    % update state estimate
    % PREDICTION:
    XbarPred=F*Xbar+B.*U;
    PPred=F*P*F'+Q;
    % CORRECTION:
    S=R+H*PPred*H';
    K=PPred*H'*inv(S);
    Xbar=XbarPred+K*(Z-H*XbarPred);
    P=(eye(2)-K*H)*PPred;

    % pause the display
    pause(1)

end

