You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
264 lines
12 KiB
264 lines
12 KiB
/**************************************************************************** |
|
* |
|
* Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. |
|
* |
|
* Redistribution and use in source and binary forms, with or without |
|
* modification, are permitted provided that the following conditions |
|
* are met: |
|
* |
|
* 1. Redistributions of source code must retain the above copyright |
|
* notice, this list of conditions and the following disclaimer. |
|
* 2. Redistributions in binary form must reproduce the above copyright |
|
* notice, this list of conditions and the following disclaimer in |
|
* the documentation and/or other materials provided with the |
|
* distribution. |
|
* 3. Neither the name ECL nor the names of its contributors may be |
|
* used to endorse or promote products derived from this software |
|
* without specific prior written permission. |
|
* |
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
|
* POSSIBILITY OF SUCH DAMAGE. |
|
* |
|
****************************************************************************/ |
|
|
|
/** |
|
* @file airspeed_fusion.cpp |
|
* airspeed fusion methods. |
|
* |
|
* @author Carl Olsson <carlolsson.co@gmail.com> |
|
* @author Roman Bast <bapstroman@gmail.com> |
|
* @author Paul Riseborough <p_riseborough@live.com.au> |
|
* |
|
*/ |
|
#include "../ecl.h" |
|
#include "ekf.h" |
|
#include <mathlib/mathlib.h> |
|
|
|
void Ekf::fuseAirspeed() |
|
{ |
|
float SH_TAS[3] = {}; // Variable used to optimise calculations of measurement jacobian |
|
float H_TAS[24] = {}; // Observation Jacobian |
|
float SK_TAS[2] = {}; // Variable used to optimise calculations of the Kalman gain vector |
|
float Kfusion[24] = {}; // Kalman gain vector |
|
|
|
const float vn = _state.vel(0); // Velocity in north direction |
|
const float ve = _state.vel(1); // Velocity in east direction |
|
const float vd = _state.vel(2); // Velocity in downwards direction |
|
const float vwn = _state.wind_vel(0); // Wind speed in north direction |
|
const float vwe = _state.wind_vel(1); // Wind speed in east direction |
|
|
|
// Calculate the predicted airspeed |
|
const float v_tas_pred = sqrtf((ve - vwe) * (ve - vwe) + (vn - vwn) * (vn - vwn) + vd * vd); |
|
|
|
// Variance for true airspeed measurement - (m/sec)^2 |
|
const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * |
|
math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f)); |
|
|
|
// Perform fusion of True Airspeed measurement |
|
if (v_tas_pred > 1.0f) { |
|
// determine if we need the sideslip fusion to correct states other than wind |
|
const bool update_wind_only = !_is_wind_dead_reckoning; |
|
|
|
// Calculate the observation jacobian |
|
// intermediate variable from algebraic optimisation |
|
SH_TAS[0] = 1.0f/v_tas_pred; |
|
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f; |
|
SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f; |
|
|
|
for (uint8_t i = 0; i < _k_num_states; i++) { H_TAS[i] = 0.0f; } |
|
|
|
H_TAS[4] = SH_TAS[2]; |
|
H_TAS[5] = SH_TAS[1]; |
|
H_TAS[6] = vd*SH_TAS[0]; |
|
H_TAS[22] = -SH_TAS[2]; |
|
H_TAS[23] = -SH_TAS[1]; |
|
|
|
// We don't want to update the innovation variance if the calculation is ill conditioned |
|
const float _airspeed_innov_var_temp = (R_TAS + SH_TAS[2]*(P(4,4)*SH_TAS[2] + P(5,4)*SH_TAS[1] - P(22,4)*SH_TAS[2] - P(23,4)*SH_TAS[1] + P(6,4)*vd*SH_TAS[0]) + SH_TAS[1]*(P(4,5)*SH_TAS[2] + P(5,5)*SH_TAS[1] - P(22,5)*SH_TAS[2] - P(23,5)*SH_TAS[1] + P(6,5)*vd*SH_TAS[0]) - SH_TAS[2]*(P(4,22)*SH_TAS[2] + P(5,22)*SH_TAS[1] - P(22,22)*SH_TAS[2] - P(23,22)*SH_TAS[1] + P(6,22)*vd*SH_TAS[0]) - SH_TAS[1]*(P(4,23)*SH_TAS[2] + P(5,23)*SH_TAS[1] - P(22,23)*SH_TAS[2] - P(23,23)*SH_TAS[1] + P(6,23)*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P(4,6)*SH_TAS[2] + P(5,6)*SH_TAS[1] - P(22,6)*SH_TAS[2] - P(23,6)*SH_TAS[1] + P(6,6)*vd*SH_TAS[0])); |
|
|
|
if (_airspeed_innov_var_temp >= R_TAS) { // Check for badly conditioned calculation |
|
SK_TAS[0] = 1.0f / _airspeed_innov_var_temp; |
|
_fault_status.flags.bad_airspeed = false; |
|
|
|
} else { // Reset the estimator covariance matrix |
|
_fault_status.flags.bad_airspeed = true; |
|
|
|
// if we are getting aiding from other sources, warn and reset the wind states and covariances only |
|
if (update_wind_only) { |
|
resetWindStates(); |
|
resetWindCovariance(); |
|
ECL_ERR_TIMESTAMPED("airspeed fusion badly conditioned - wind covariance reset"); |
|
|
|
} else { |
|
initialiseCovariance(); |
|
_state.wind_vel.setZero(); |
|
ECL_ERR_TIMESTAMPED("airspeed fusion badly conditioned - full covariance reset"); |
|
} |
|
|
|
return; |
|
} |
|
|
|
SK_TAS[1] = SH_TAS[1]; |
|
|
|
if (update_wind_only) { |
|
// If we are getting aiding from other sources, then don't allow the airspeed measurements to affect the non-windspeed states |
|
for (unsigned row = 0; row <= 21; row++) { |
|
Kfusion[row] = 0.0f; |
|
} |
|
|
|
} else { |
|
// we have no other source of aiding, so use airspeed measurements to correct states |
|
Kfusion[0] = SK_TAS[0]*(P(0,4)*SH_TAS[2] - P(0,22)*SH_TAS[2] + P(0,5)*SK_TAS[1] - P(0,23)*SK_TAS[1] + P(0,6)*vd*SH_TAS[0]); |
|
Kfusion[1] = SK_TAS[0]*(P(1,4)*SH_TAS[2] - P(1,22)*SH_TAS[2] + P(1,5)*SK_TAS[1] - P(1,23)*SK_TAS[1] + P(1,6)*vd*SH_TAS[0]); |
|
Kfusion[2] = SK_TAS[0]*(P(2,4)*SH_TAS[2] - P(2,22)*SH_TAS[2] + P(2,5)*SK_TAS[1] - P(2,23)*SK_TAS[1] + P(2,6)*vd*SH_TAS[0]); |
|
Kfusion[3] = SK_TAS[0]*(P(3,4)*SH_TAS[2] - P(3,22)*SH_TAS[2] + P(3,5)*SK_TAS[1] - P(3,23)*SK_TAS[1] + P(3,6)*vd*SH_TAS[0]); |
|
Kfusion[4] = SK_TAS[0]*(P(4,4)*SH_TAS[2] - P(4,22)*SH_TAS[2] + P(4,5)*SK_TAS[1] - P(4,23)*SK_TAS[1] + P(4,6)*vd*SH_TAS[0]); |
|
Kfusion[5] = SK_TAS[0]*(P(5,4)*SH_TAS[2] - P(5,22)*SH_TAS[2] + P(5,5)*SK_TAS[1] - P(5,23)*SK_TAS[1] + P(5,6)*vd*SH_TAS[0]); |
|
Kfusion[6] = SK_TAS[0]*(P(6,4)*SH_TAS[2] - P(6,22)*SH_TAS[2] + P(6,5)*SK_TAS[1] - P(6,23)*SK_TAS[1] + P(6,6)*vd*SH_TAS[0]); |
|
Kfusion[7] = SK_TAS[0]*(P(7,4)*SH_TAS[2] - P(7,22)*SH_TAS[2] + P(7,5)*SK_TAS[1] - P(7,23)*SK_TAS[1] + P(7,6)*vd*SH_TAS[0]); |
|
Kfusion[8] = SK_TAS[0]*(P(8,4)*SH_TAS[2] - P(8,22)*SH_TAS[2] + P(8,5)*SK_TAS[1] - P(8,23)*SK_TAS[1] + P(8,6)*vd*SH_TAS[0]); |
|
Kfusion[9] = SK_TAS[0]*(P(9,4)*SH_TAS[2] - P(9,22)*SH_TAS[2] + P(9,5)*SK_TAS[1] - P(9,23)*SK_TAS[1] + P(9,6)*vd*SH_TAS[0]); |
|
Kfusion[10] = SK_TAS[0]*(P(10,4)*SH_TAS[2] - P(10,22)*SH_TAS[2] + P(10,5)*SK_TAS[1] - P(10,23)*SK_TAS[1] + P(10,6)*vd*SH_TAS[0]); |
|
Kfusion[11] = SK_TAS[0]*(P(11,4)*SH_TAS[2] - P(11,22)*SH_TAS[2] + P(11,5)*SK_TAS[1] - P(11,23)*SK_TAS[1] + P(11,6)*vd*SH_TAS[0]); |
|
Kfusion[12] = SK_TAS[0]*(P(12,4)*SH_TAS[2] - P(12,22)*SH_TAS[2] + P(12,5)*SK_TAS[1] - P(12,23)*SK_TAS[1] + P(12,6)*vd*SH_TAS[0]); |
|
Kfusion[13] = SK_TAS[0]*(P(13,4)*SH_TAS[2] - P(13,22)*SH_TAS[2] + P(13,5)*SK_TAS[1] - P(13,23)*SK_TAS[1] + P(13,6)*vd*SH_TAS[0]); |
|
Kfusion[14] = SK_TAS[0]*(P(14,4)*SH_TAS[2] - P(14,22)*SH_TAS[2] + P(14,5)*SK_TAS[1] - P(14,23)*SK_TAS[1] + P(14,6)*vd*SH_TAS[0]); |
|
Kfusion[15] = SK_TAS[0]*(P(15,4)*SH_TAS[2] - P(15,22)*SH_TAS[2] + P(15,5)*SK_TAS[1] - P(15,23)*SK_TAS[1] + P(15,6)*vd*SH_TAS[0]); |
|
Kfusion[16] = SK_TAS[0]*(P(16,4)*SH_TAS[2] - P(16,22)*SH_TAS[2] + P(16,5)*SK_TAS[1] - P(16,23)*SK_TAS[1] + P(16,6)*vd*SH_TAS[0]); |
|
Kfusion[17] = SK_TAS[0]*(P(17,4)*SH_TAS[2] - P(17,22)*SH_TAS[2] + P(17,5)*SK_TAS[1] - P(17,23)*SK_TAS[1] + P(17,6)*vd*SH_TAS[0]); |
|
Kfusion[18] = SK_TAS[0]*(P(18,4)*SH_TAS[2] - P(18,22)*SH_TAS[2] + P(18,5)*SK_TAS[1] - P(18,23)*SK_TAS[1] + P(18,6)*vd*SH_TAS[0]); |
|
Kfusion[19] = SK_TAS[0]*(P(19,4)*SH_TAS[2] - P(19,22)*SH_TAS[2] + P(19,5)*SK_TAS[1] - P(19,23)*SK_TAS[1] + P(19,6)*vd*SH_TAS[0]); |
|
Kfusion[20] = SK_TAS[0]*(P(20,4)*SH_TAS[2] - P(20,22)*SH_TAS[2] + P(20,5)*SK_TAS[1] - P(20,23)*SK_TAS[1] + P(20,6)*vd*SH_TAS[0]); |
|
Kfusion[21] = SK_TAS[0]*(P(21,4)*SH_TAS[2] - P(21,22)*SH_TAS[2] + P(21,5)*SK_TAS[1] - P(21,23)*SK_TAS[1] + P(21,6)*vd*SH_TAS[0]); |
|
} |
|
Kfusion[22] = SK_TAS[0]*(P(22,4)*SH_TAS[2] - P(22,22)*SH_TAS[2] + P(22,5)*SK_TAS[1] - P(22,23)*SK_TAS[1] + P(22,6)*vd*SH_TAS[0]); |
|
Kfusion[23] = SK_TAS[0]*(P(23,4)*SH_TAS[2] - P(23,22)*SH_TAS[2] + P(23,5)*SK_TAS[1] - P(23,23)*SK_TAS[1] + P(23,6)*vd*SH_TAS[0]); |
|
|
|
|
|
// Calculate measurement innovation |
|
_airspeed_innov = v_tas_pred - |
|
_airspeed_sample_delayed.true_airspeed; |
|
|
|
// Calculate the innovation variance |
|
_airspeed_innov_var = 1.0f / SK_TAS[0]; |
|
|
|
// Compute the ratio of innovation to gate size |
|
_tas_test_ratio = sq(_airspeed_innov) / (sq(fmaxf(_params.tas_innov_gate, 1.0f)) * _airspeed_innov_var); |
|
|
|
// If the innovation consistency check fails then don't fuse the sample and indicate bad airspeed health |
|
if (_tas_test_ratio > 1.0f) { |
|
_innov_check_fail_status.flags.reject_airspeed = true; |
|
return; |
|
|
|
} else { |
|
_innov_check_fail_status.flags.reject_airspeed = false; |
|
} |
|
|
|
// Airspeed measurement sample has passed check so record it |
|
_time_last_arsp_fuse = _time_last_imu; |
|
|
|
// apply covariance correction via P_new = (I -K*H)*P |
|
// first calculate expression for KHP |
|
// then calculate P - KHP |
|
matrix::SquareMatrix<float, _k_num_states> KHP; |
|
float KH[5]; |
|
|
|
for (unsigned row = 0; row < _k_num_states; row++) { |
|
|
|
KH[0] = Kfusion[row] * H_TAS[4]; |
|
KH[1] = Kfusion[row] * H_TAS[5]; |
|
KH[2] = Kfusion[row] * H_TAS[6]; |
|
KH[3] = Kfusion[row] * H_TAS[22]; |
|
KH[4] = Kfusion[row] * H_TAS[23]; |
|
|
|
for (unsigned column = 0; column < _k_num_states; column++) { |
|
float tmp = KH[0] * P(4,column); |
|
tmp += KH[1] * P(5,column); |
|
tmp += KH[2] * P(6,column); |
|
tmp += KH[3] * P(22,column); |
|
tmp += KH[4] * P(23,column); |
|
KHP(row,column) = tmp; |
|
} |
|
} |
|
|
|
// if the covariance correction will result in a negative variance, then |
|
// the covariance matrix is unhealthy and must be corrected |
|
bool healthy = true; |
|
_fault_status.flags.bad_airspeed = false; |
|
|
|
for (int i = 0; i < _k_num_states; i++) { |
|
if (P(i,i) < KHP(i,i)) { |
|
// zero rows and columns |
|
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f); |
|
|
|
//flag as unhealthy |
|
healthy = false; |
|
|
|
// update individual measurement health status |
|
_fault_status.flags.bad_airspeed = true; |
|
|
|
} |
|
} |
|
|
|
// only apply covariance and state corrections if healthy |
|
if (healthy) { |
|
// apply the covariance corrections |
|
P = P - KHP; |
|
|
|
// correct the covariance matrix for gross errors |
|
fixCovarianceErrors(true); |
|
|
|
// apply the state corrections |
|
fuse(Kfusion, _airspeed_innov); |
|
|
|
} |
|
} |
|
} |
|
|
|
void Ekf::get_wind_velocity(float *wind) |
|
{ |
|
wind[0] = _state.wind_vel(0); |
|
wind[1] = _state.wind_vel(1); |
|
} |
|
|
|
void Ekf::get_wind_velocity_var(float *wind_var) |
|
{ |
|
wind_var[0] = P(22,22); |
|
wind_var[1] = P(23,23); |
|
} |
|
|
|
void Ekf::get_true_airspeed(float *tas) |
|
{ |
|
float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(_state.vel(2))); |
|
memcpy(tas, &tempvar, sizeof(float)); |
|
} |
|
|
|
/* |
|
* Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip |
|
*/ |
|
void Ekf::resetWindStates() |
|
{ |
|
// get euler yaw angle |
|
Eulerf euler321(_state.quat_nominal); |
|
const float euler_yaw = euler321(2); |
|
|
|
if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) { |
|
// estimate wind using zero sideslip assumption and airspeed measurement if airspeed available |
|
_state.wind_vel(0) = _state.vel(0) - _airspeed_sample_delayed.true_airspeed * cosf(euler_yaw); |
|
_state.wind_vel(1) = _state.vel(1) - _airspeed_sample_delayed.true_airspeed * sinf(euler_yaw); |
|
|
|
} else { |
|
// If we don't have an airspeed measurement, then assume the wind is zero |
|
_state.wind_vel.setZero(); |
|
} |
|
}
|
|
|