% Implementation of a simple 3-state EKF that can identify the scale
% factor that needs to be applied to a true airspeed measurement
% Paul Riseborough 27 June 2013

% Inputs:
% Measured true airsped (m/s)

clear all;

% Define wind speed used for truth model
vwn_truth = 4.0;
vwe_truth = 3.0;
vwd_truth = -0.5; % convection can produce values of up to 1.5 m/s, however 
                  % average will zero over longer periods at lower altitudes
                  % Slope lift will be persistent

% Define airspeed scale factor used for truth model
K_truth = 1.2;

% Use a 1 second time step
DT = 1.0;

% Define the initial state error covariance matrix
% Assume initial wind uncertainty of 10 m/s and scale factor uncertainty of
% 0.2
P = diag([10^2 10^2 0.001^2]);

% Define state error growth matrix assuming wind changes at a rate of 0.1
% m/s/s and scale factor drifts at a rate of 0.001 per second
Q = diag([0.1^2 0.1^2 0.001^2])*DT^2;

% Define the initial state matrix assuming zero wind and a scale factor of
% 1.0
x = [0;0;1.0];

for i = 1:1000
    
    %% Calculate truth values
    % calculate ground velocity by simulating a wind relative
    % circular path of of 60m radius and 16 m/s airspeed
    time = i*DT;
    radius = 60;
    TAS_truth = 16;
    vwnrel_truth = TAS_truth*cos(TAS_truth*time/radius);
    vwerel_truth = TAS_truth*sin(TAS_truth*time/radius);
    vwdrel_truth = 0.0;
    vgn_truth = vwnrel_truth + vwn_truth;
    vge_truth = vwerel_truth + vwe_truth;
    vgd_truth = vwdrel_truth + vwd_truth;
    
    % calculate measured ground velocity and airspeed, adding some noise and
    % adding a scale factor to the airspeed measurement.
    vgn_mea = vgn_truth + 0.1*rand;
    vge_mea = vge_truth + 0.1*rand;
    vgd_mea = vgd_truth + 0.1*rand;
    TAS_mea = K_truth * TAS_truth + 0.5*rand;
    
    %% Perform filter processing
    % This benefits from a matrix library that can handle up to 3x3
    % matrices
    
    % Perform the covariance prediction
    % Q is a diagonal matrix so only need to add three terms in
    % C code implementation
    P = P + Q;
    
    % Perform the predicted measurement using the current state estimates
    % No state prediction required because states are assumed to be time
    % invariant plus process noise
    % Ignore vertical wind component
    TAS_pred = x(3) * sqrt((vgn_mea - x(1))^2 + (vge_mea - x(2))^2 + vgd_mea^2);
    
    % Calculate the observation Jacobian H_TAS
    SH1 = (vge_mea - x(2))^2 + (vgn_mea - x(1))^2;
    SH2 = 1/sqrt(SH1);
    H_TAS = zeros(1,3);
    H_TAS(1,1) = -(x(3)*SH2*(2*vgn_mea - 2*x(1)))/2;
    H_TAS(1,2) = -(x(3)*SH2*(2*vge_mea - 2*x(2)))/2;
    H_TAS(1,3) = 1/SH2;
    
    % Calculate the fusion innovaton covariance assuming a TAS measurement
    % noise of 1.0 m/s
    S = H_TAS*P*H_TAS' + 1.0; % [1 x 3] * [3 x 3] * [3 x 1] + [1 x 1]
    
    % Calculate the Kalman gain
    KG = P*H_TAS'/S; % [3 x 3] * [3 x 1] / [1 x 1]
    
    % Update the states
    x = x + KG*(TAS_mea - TAS_pred); % [3 x 1] + [3 x 1] * [1 x 1]
    
    % Update the covariance matrix
    P = P - KG*H_TAS*P; % [3 x 3] *
    
    % force symmetry on the covariance matrix - necessary due to rounding
    % errors
    % Implementation will also need a further check to prevent diagonal 
    % terms becoming negative due to rounding errors
    % This step can be made more efficient by excluding diagonal terms 
    % (would reduce processing by 1/3)
    P = 0.5*(P + P'); % [1 x 1] * ( [3 x 3] + [3 x 3])
    
    %% Store results
    output(i,:) = [time,x(1),x(2),x(3),vwn_truth,vwe_truth,K_truth];
   
end

%% Plot output
figure;
subplot(3,1,1);plot(output(:,1),[output(:,2),output(:,5)]);
ylabel('Wind Vel North (m/s)');
xlabel('time (sec)');
grid on;
subplot(3,1,2);plot(output(:,1),[output(:,3),output(:,6)]);
ylabel('Wind Vel East (m/s)');
xlabel('time (sec)');
grid on;
subplot(3,1,3);plot(output(:,1),[output(:,4),output(:,7)]);
ylim([0 1.5]);
ylabel('Airspeed scale factor correction');
xlabel('time (sec)');
grid on;