%%
%% Extended Kalman filter
%%
%% We use the pvtol_extsim model to do a extended Kalman filter design
%%

aminit;
amuse('pvtol');
pvtol_params;			% System parameters

% Global variables used by the EKF model
global pvtol_K pvtol_L pvtol_Rv pvtol_Rw pvtol_C pvtol_F

% System matrices (entire plant: 2 input, 2 output)
[A, B, C, D] = pvtol_linearize([0 0 0 0 0 0], [0 m*g]);

% Reset the output to be position and orientation
pvtol_C = [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 1 0 0 0];
pvtol_D = zeros(3);

% Controller
pvtol_K = lqr(A, B, eye(size(A)), 0.1*eye(size(B'*B)));

% Estimator
pvtol_F = eye(6);			% disturbances/model error
pvtol_Rv = diag([0.0001, 0.0001, 0.0001, 0.01, 0.04, 0.0001]);

R11 = 0.1; R22 = 0.1; R33 = 0.001;
pvtol_Rw = diag([R11 R22 R33]);

pvtol_L = lqe(A, pvtol_F, pvtol_C, pvtol_Rv, pvtol_Rw);

x0 = [0.1 0.2 0 0 0 0];
[t2, y2] = ode45(@pvtol_ekf, [0 10], [x0, x0*2, ones(1,36)]);

figure(8); clf
subplot(321);
  plot(t2, y2(:,1), 'b-', t2, y2(:,2), 'g--', t2, y2(:,3), 'r--');
  legend x y th;
  xlabel('time');
  ylabel('States (w/ \theta)');
  axis([0 15 -0.3 0.3]);

subplot(323);
  plot(t2, y2(:,7) - y2(:,1), 'b-', ...
       t2, y2(:,8) - y2(:,2), 'g--', ...
       t2, y2(:,9) - y2(:,3), 'r-');
  legend xerr yerr terr;
  xlabel('time');
  ylabel('Errors (w/ \theta)');
  axis([0 15 -0.2 0.2]);

subplot(325);
  plot(t2, y2(:,13), 'b-', t2, y2(:,19), 'g--', t2, y2(:,25), 'r-');
  legend P11 P22 P33
  xlabel('time');
  ylabel('Covariance (w/ \theta)');
  axis([0 15 -0.1 0.3]);
