% Carrick Detweiler
% 439/839 Kalman Filter example
%
% Estimates pos, vel, and acceleration
% Measures pos and accel

clear;

time = (0:99)';
accel = rand(size(time))*.2 - .1;
vel(1) = 0;
pos(1) = 0;
z(1,:) = [pos(1) accel(1)];
for i=2:size(time)
    vel(i) = vel(i-1) + accel(i);
    pos(i) = pos(i-1) + vel(i);
    z(i,:) = [pos(i)+(2*(rand() - .5)) accel(i)+(.0001*(rand() - .5))];
end

% Kalman Vars
A = [1 1 0.5
    0 1 1
    0 0 1];  % Related prior state to current state
H = [1 0 0
     0 0 1];  % Relates state to measurement

B = [0];  % Maps control input u to state x

Q = [.01 0 0
      0 .001 0 
      0 0 .1]; % Process noise covariance 
R = [1.8 0; 
     0 0.0]; % Measurement noise covariance

% Kalman Init
x = [z(1,1) 0 z(1,2)]';
x_m = [z(1,1) 0 z(1,2)]';
x_hist(1,:) = [z(1,1) 0 z(1,2)]';
% P is prior error covariance (error in est of x before integrating new
% measurement)
P = [0 0 0; 0 0 0; 0 0 0];
P_m = [0 0 0; 0 0 0; 0 0 0];
for k=2:size(time)
    % Time update
    x_m = A*x;% + B*u(i-1);
    P_m = A*P*A' + Q;
    
    % Measurement update
    K = P_m * H' * inv(H*P_m*H' + R);
    x = x_m + K * (z(k,:)' - H * x_m);
    P = (eye(size(H,2)) - K*H)*P_m;
    
    x_hist(k,:) = x;
end


% Plot
figure(1);
clf;
hold on;
plot(pos,'k');
plot(z(:,1),'g');
plot(x_hist(:,1),'b');
legend('true pos','pos measurement','pos kalman');

figure(2);
clf;
hold on;
plot(vel,'k');
plot(x_hist(:,2),'b');
legend('vel true', 'vel kalman');


figure(3);
clf;
hold on;
plot(accel,'k');
plot(z(:,2),'g');
plot(x_hist(:,3),'b');
legend('accel true','accel measurement','accel kalman');

