zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Airspeed: fixup line endings

master
Kevin Hester 11 years ago committed by Randy Mackay
parent
c34c4d79f6
commit
d09e871319
1 changed files with 122 additions and 122 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 244
      libraries/AP_Airspeed/models/ADS_cal_EKF.m

244
libraries/AP_Airspeed/models/ADS_cal_EKF.m
Unescape View File

@ -1,122 +1,122 @@ @@ -1,122 +1,122 @@
% 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;
% 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;

Write Preview
Loading…
Cancel
Save