From d09e871319075251942462b866801a7e34c5200f Mon Sep 17 00:00:00 2001 From: Kevin Hester Date: Tue, 15 Apr 2014 15:50:57 +0900 Subject: [PATCH] AP_Airspeed: fixup line endings --- libraries/AP_Airspeed/models/ADS_cal_EKF.m | 244 ++++++++++----------- 1 file changed, 122 insertions(+), 122 deletions(-) diff --git a/libraries/AP_Airspeed/models/ADS_cal_EKF.m b/libraries/AP_Airspeed/models/ADS_cal_EKF.m index 0c5def09dd..ee7667e321 100644 --- a/libraries/AP_Airspeed/models/ADS_cal_EKF.m +++ b/libraries/AP_Airspeed/models/ADS_cal_EKF.m @@ -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; +