ardupilot/libraries/AP_Airspeed/models/ADS_cal_EKF.m

123 lines
3.9 KiB
Matlab

% 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;