% pvtol_ekf.m - nonlinear PVTOL model, with LQR and EKF
% RMM, 5 Feb 06
%
% This function has the dynamics for a nonlinear planar vertical
% takeoff and landing aircraft, with an LQR compensator and EKF
% estimator.  The model includes noise and disturbance forces.
%
% state(1)	x position, in meters
% state(2)	y position, in meters
% state(3)	theta angle, in radians
% state(4-6)	velocities
% state(7-12)	estimated states
% state(13-48)	covariance matrix (ordered rowise)

function deriv = pvtol_ekf(t, state, flags)
global pvtol_K;				% LQR gain
global pvtol_L;				% LQE gain (temporary)
global pvtol_Rv;			% Disturbance covariance
global pvtol_Rw;			% Noise covariance
global pvtol_C;				% outputs to use
global pvtol_F;				% disturbance input

pvtol_params;				% Load system parameters

% Initialize the derivative so that it is correct size and shape
deriv = zeros(size(state));

% Extract the current state estimate
x = state(1:6);
xhat = state(7:12);

% Get the current output, with noise
y = pvtol_C*x + pvtol_C * ...
  [0.1*sin(2.1*t); 0.1*sin(3.2*t); 0; 0; 0; 0];

% Compute the disturbance forces
fd = [
  0.01*sin(0.1*t); 0.01*cos(0.027*t); 0
];

% Compute the control law
F = -pvtol_K * xhat + [0; m*g];

% Get dynamics at the current equilibrium point
[A, B, C, D] = pvtol_linearize(xhat, F);

% Estimator dynamics (prediction)
deriv(7) = xhat(4); deriv(8) = xhat(5); deriv(9) = xhat(6);
deriv(10) = (F(1) * cos(xhat(3)) - F(2) * sin(xhat(3)) - c*xhat(4)) / m;
deriv(11) = (F(1) * sin(xhat(3)) + F(2) * cos(xhat(3)) - m*g - c*xhat(5)) / m;
deriv(12) = (F(1) * r) / J;

% Compute the covariance
P = reshape(state(13:48), 6, 6);
dP = A * P + P * A' - P * pvtol_C' * inv(pvtol_Rw) * pvtol_C * P + ...
  pvtol_F * pvtol_Rv * pvtol_F';
L = P * pvtol_C' * inv(pvtol_Rw);

% Now compute correction
xcor = L * (y - pvtol_C*xhat);
for i = 1:6, deriv(6+i) = deriv(6+i) + xcor(i); end;

% PVTOL dynamics
deriv(1) = x(4); deriv(2) = x(5); deriv(3) = x(6);
deriv(4) = (F(1)*cos(x(3)) - F(2)*sin(x(3)) - c*x(4) + fd(1)) / m;
deriv(5) = (F(1)*sin(x(3)) + F(2)*cos(x(3)) - m*g - c*x(5) + fd(2)) / m;
deriv(6) = (F(1) * r + fd(3)) / J;

% Copy in the covariance updates
for i = 1:6,
  for j = 1:6,
    deriv(6+6*i+j) = dP(i, j);
  end;
end;

% All done
return;
