forked from Archive/PX4-Autopilot
1952 lines
66 KiB
C++
1952 lines
66 KiB
C++
/****************************************************************************
|
|
*
|
|
* 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 ekf_helper.cpp
|
|
* Definition of ekf helper functions.
|
|
*
|
|
* @author Roman Bast <bapstroman@gmail.com>
|
|
*
|
|
*/
|
|
|
|
#include "ekf.h"
|
|
|
|
#include <ecl.h>
|
|
#include <mathlib/mathlib.h>
|
|
#include <cstdlib>
|
|
|
|
// Reset the velocity states. If we have a recent and valid
|
|
// gps measurement then use for velocity initialisation
|
|
bool Ekf::resetVelocity()
|
|
{
|
|
// used to calculate the velocity change due to the reset
|
|
Vector3f vel_before_reset = _state.vel;
|
|
|
|
// reset EKF states
|
|
if (_control_status.flags.gps && _gps_check_fail_status.value==0) {
|
|
ECL_INFO_TIMESTAMPED("EKF reset velocity to GPS");
|
|
// this reset is only called if we have new gps data at the fusion time horizon
|
|
_state.vel = _gps_sample_delayed.vel;
|
|
|
|
// use GPS accuracy to reset variances
|
|
setDiag(P, 4, 6, sq(_gps_sample_delayed.sacc));
|
|
|
|
} else if (_control_status.flags.opt_flow) {
|
|
ECL_INFO_TIMESTAMPED("EKF reset velocity to flow");
|
|
// constrain height above ground to be above minimum possible
|
|
float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
|
|
|
|
// calculate absolute distance from focal point to centre of frame assuming a flat earth
|
|
float range = heightAboveGndEst / _R_rng_to_earth_2_2;
|
|
|
|
if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
|
|
// we should have reliable OF measurements so
|
|
// calculate X and Y body relative velocities from OF measurements
|
|
Vector3f vel_optflow_body;
|
|
vel_optflow_body(0) = - range * _flowRadXYcomp(1) / _flow_sample_delayed.dt;
|
|
vel_optflow_body(1) = range * _flowRadXYcomp(0) / _flow_sample_delayed.dt;
|
|
vel_optflow_body(2) = 0.0f;
|
|
|
|
// rotate from body to earth frame
|
|
Vector3f vel_optflow_earth;
|
|
vel_optflow_earth = _R_to_earth * vel_optflow_body;
|
|
|
|
// take x and Y components
|
|
_state.vel(0) = vel_optflow_earth(0);
|
|
_state.vel(1) = vel_optflow_earth(1);
|
|
|
|
} else {
|
|
_state.vel(0) = 0.0f;
|
|
_state.vel(1) = 0.0f;
|
|
}
|
|
|
|
// reset the velocity covariance terms
|
|
zeroRows(P, 4, 5);
|
|
zeroCols(P, 4, 5);
|
|
|
|
// reset the horizontal velocity variance using the optical flow noise variance
|
|
P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar();
|
|
} else if (_control_status.flags.ev_vel) {
|
|
ECL_INFO_TIMESTAMPED("EKF reset velocity to ev velocity");
|
|
Vector3f _ev_vel = _ev_sample_delayed.vel;
|
|
if(_params.fusion_mode & MASK_ROTATE_EV){
|
|
_ev_vel = _ev_rot_mat *_ev_sample_delayed.vel;
|
|
}
|
|
_state.vel(0) = _ev_vel(0);
|
|
_state.vel(1) = _ev_vel(1);
|
|
_state.vel(2) = _ev_vel(2);
|
|
setDiag(P, 4, 6, sq(_ev_sample_delayed.velErr));
|
|
} else {
|
|
ECL_INFO_TIMESTAMPED("EKF reset velocity to zero");
|
|
// Used when falling back to non-aiding mode of operation
|
|
_state.vel(0) = 0.0f;
|
|
_state.vel(1) = 0.0f;
|
|
setDiag(P, 4, 5, 25.0f);
|
|
}
|
|
|
|
// calculate the change in velocity and apply to the output predictor state history
|
|
const Vector3f velocity_change = _state.vel - vel_before_reset;
|
|
|
|
for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
|
|
_output_buffer[index].vel += velocity_change;
|
|
}
|
|
|
|
// apply the change in velocity to our newest velocity estimate
|
|
// which was already taken out from the output buffer
|
|
_output_new.vel += velocity_change;
|
|
|
|
// capture the reset event
|
|
_state_reset_status.velNE_change(0) = velocity_change(0);
|
|
_state_reset_status.velNE_change(1) = velocity_change(1);
|
|
_state_reset_status.velD_change = velocity_change(2);
|
|
_state_reset_status.velNE_counter++;
|
|
_state_reset_status.velD_counter++;
|
|
|
|
return true;
|
|
}
|
|
|
|
// Reset position states. If we have a recent and valid
|
|
// gps measurement then use for position initialisation
|
|
bool Ekf::resetPosition()
|
|
{
|
|
// ECL_INFO_TIMESTAMPED("Reset Position");
|
|
// used to calculate the position change due to the reset
|
|
Vector2f posNE_before_reset;
|
|
posNE_before_reset(0) = _state.pos(0);
|
|
posNE_before_reset(1) = _state.pos(1);
|
|
|
|
// let the next odometry update know that the previous value of states cannot be used to calculate the change in position
|
|
_hpos_prev_available = false;
|
|
|
|
if (_control_status.flags.gps) {
|
|
ECL_INFO_TIMESTAMPED("EKF reset position to GPS");
|
|
|
|
// this reset is only called if we have new gps data at the fusion time horizon
|
|
_state.pos(0) = _gps_sample_delayed.pos(0);
|
|
_state.pos(1) = _gps_sample_delayed.pos(1);
|
|
|
|
// use GPS accuracy to reset variances
|
|
setDiag(P, 7, 8, sq(_gps_sample_delayed.hacc));
|
|
|
|
} else if (_control_status.flags.ev_pos) {
|
|
ECL_INFO_TIMESTAMPED("EKF reset position to ev position");
|
|
|
|
// this reset is only called if we have new ev data at the fusion time horizon
|
|
Vector3f _ev_pos = _ev_sample_delayed.pos;
|
|
if(_params.fusion_mode & MASK_ROTATE_EV){
|
|
_ev_pos = _ev_rot_mat *_ev_sample_delayed.pos;
|
|
}
|
|
_state.pos(0) = _ev_pos(0);
|
|
_state.pos(1) = _ev_pos(1);
|
|
|
|
// use EV accuracy to reset variances
|
|
setDiag(P, 7, 8, sq(_ev_sample_delayed.posErr));
|
|
|
|
} else if (_control_status.flags.opt_flow) {
|
|
ECL_INFO_TIMESTAMPED("EKF reset position to last known position");
|
|
|
|
if (!_control_status.flags.in_air) {
|
|
// we are likely starting OF for the first time so reset the horizontal position
|
|
_state.pos(0) = 0.0f;
|
|
_state.pos(1) = 0.0f;
|
|
|
|
} else {
|
|
// set to the last known position
|
|
_state.pos(0) = _last_known_posNE(0);
|
|
_state.pos(1) = _last_known_posNE(1);
|
|
|
|
}
|
|
|
|
// estimate is relative to initial position in this mode, so we start with zero error.
|
|
zeroCols(P,7,8);
|
|
zeroRows(P,7,8);
|
|
|
|
} else {
|
|
ECL_INFO_TIMESTAMPED("EKF reset position to last known position");
|
|
// Used when falling back to non-aiding mode of operation
|
|
_state.pos(0) = _last_known_posNE(0);
|
|
_state.pos(1) = _last_known_posNE(1);
|
|
setDiag(P, 7, 8, sq(_params.pos_noaid_noise));
|
|
}
|
|
|
|
// calculate the change in position and apply to the output predictor state history
|
|
const Vector2f posNE_change{_state.pos(0) - posNE_before_reset(0), _state.pos(1) - posNE_before_reset(1)};
|
|
|
|
for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
|
|
_output_buffer[index].pos(0) += posNE_change(0);
|
|
_output_buffer[index].pos(1) += posNE_change(1);
|
|
}
|
|
|
|
// apply the change in position to our newest position estimate
|
|
// which was already taken out from the output buffer
|
|
_output_new.pos(0) += posNE_change(0);
|
|
_output_new.pos(1) += posNE_change(1);
|
|
|
|
// capture the reset event
|
|
_state_reset_status.posNE_change = posNE_change;
|
|
_state_reset_status.posNE_counter++;
|
|
|
|
return true;
|
|
}
|
|
|
|
// Reset height state using the last height measurement
|
|
void Ekf::resetHeight()
|
|
{
|
|
// Get the most recent GPS data
|
|
const gpsSample &gps_newest = _gps_buffer.get_newest();
|
|
|
|
// store the current vertical position and velocity for reference so we can calculate and publish the reset amount
|
|
float old_vert_pos = _state.pos(2);
|
|
bool vert_pos_reset = false;
|
|
float old_vert_vel = _state.vel(2);
|
|
bool vert_vel_reset = false;
|
|
|
|
// reset the vertical position
|
|
if (_control_status.flags.rng_hgt) {
|
|
|
|
float new_pos_down = _hgt_sensor_offset - _range_sample_delayed.rng * _R_rng_to_earth_2_2;
|
|
|
|
// update the state and associated variance
|
|
_state.pos(2) = new_pos_down;
|
|
|
|
// reset the associated covariance values
|
|
zeroRows(P, 9, 9);
|
|
zeroCols(P, 9, 9);
|
|
|
|
// the state variance is the same as the observation
|
|
P[9][9] = sq(_params.range_noise);
|
|
|
|
vert_pos_reset = true;
|
|
|
|
// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
|
|
const baroSample &baro_newest = _baro_buffer.get_newest();
|
|
_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
|
|
|
|
} else if (_control_status.flags.baro_hgt) {
|
|
// initialize vertical position with newest baro measurement
|
|
const baroSample &baro_newest = _baro_buffer.get_newest();
|
|
|
|
if (_time_last_imu - baro_newest.time_us < 2 * BARO_MAX_INTERVAL) {
|
|
_state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset;
|
|
|
|
// reset the associated covariance values
|
|
zeroRows(P, 9, 9);
|
|
zeroCols(P, 9, 9);
|
|
|
|
// the state variance is the same as the observation
|
|
P[9][9] = sq(_params.baro_noise);
|
|
|
|
vert_pos_reset = true;
|
|
|
|
} else {
|
|
// TODO: reset to last known baro based estimate
|
|
}
|
|
|
|
} else if (_control_status.flags.gps_hgt) {
|
|
// initialize vertical position and velocity with newest gps measurement
|
|
if (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) {
|
|
_state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref;
|
|
|
|
// reset the associated covariance values
|
|
zeroRows(P, 9, 9);
|
|
zeroCols(P, 9, 9);
|
|
|
|
// the state variance is the same as the observation
|
|
P[9][9] = sq(gps_newest.hacc);
|
|
|
|
vert_pos_reset = true;
|
|
|
|
// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
|
|
const baroSample &baro_newest = _baro_buffer.get_newest();
|
|
_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
|
|
|
|
} else {
|
|
// TODO: reset to last known gps based estimate
|
|
}
|
|
|
|
} else if (_control_status.flags.ev_hgt) {
|
|
// initialize vertical position with newest measurement
|
|
const extVisionSample &ev_newest = _ext_vision_buffer.get_newest();
|
|
|
|
// use the most recent data if it's time offset from the fusion time horizon is smaller
|
|
int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
|
|
int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us;
|
|
|
|
vert_pos_reset = true;
|
|
|
|
if (std::abs(dt_newest) < std::abs(dt_delayed)) {
|
|
_state.pos(2) = ev_newest.pos(2);
|
|
|
|
} else {
|
|
_state.pos(2) = _ev_sample_delayed.pos(2);
|
|
}
|
|
|
|
}
|
|
|
|
// reset the vertical velocity covariance values
|
|
zeroRows(P, 6, 6);
|
|
zeroCols(P, 6, 6);
|
|
|
|
// reset the vertical velocity state
|
|
if (_control_status.flags.gps && (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL)) {
|
|
// If we are using GPS, then use it to reset the vertical velocity
|
|
_state.vel(2) = gps_newest.vel(2);
|
|
|
|
// the state variance is the same as the observation
|
|
P[6][6] = sq(1.5f * gps_newest.sacc);
|
|
|
|
} else {
|
|
// we don't know what the vertical velocity is, so set it to zero
|
|
_state.vel(2) = 0.0f;
|
|
|
|
// Set the variance to a value large enough to allow the state to converge quickly
|
|
// that does not destabilise the filter
|
|
P[6][6] = 10.0f;
|
|
|
|
}
|
|
|
|
vert_vel_reset = true;
|
|
|
|
// store the reset amount and time to be published
|
|
if (vert_pos_reset) {
|
|
_state_reset_status.posD_change = _state.pos(2) - old_vert_pos;
|
|
_state_reset_status.posD_counter++;
|
|
}
|
|
|
|
if (vert_vel_reset) {
|
|
_state_reset_status.velD_change = _state.vel(2) - old_vert_vel;
|
|
_state_reset_status.velD_counter++;
|
|
}
|
|
|
|
// apply the change in height / height rate to our newest height / height rate estimate
|
|
// which have already been taken out from the output buffer
|
|
if (vert_pos_reset) {
|
|
_output_new.pos(2) += _state_reset_status.posD_change;
|
|
}
|
|
|
|
if (vert_vel_reset) {
|
|
_output_new.vel(2) += _state_reset_status.velD_change;
|
|
}
|
|
|
|
// add the reset amount to the output observer buffered data
|
|
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
|
|
if (vert_pos_reset) {
|
|
_output_buffer[i].pos(2) += _state_reset_status.posD_change;
|
|
_output_vert_buffer[i].vel_d_integ += _state_reset_status.posD_change;
|
|
}
|
|
|
|
if (vert_vel_reset) {
|
|
_output_buffer[i].vel(2) += _state_reset_status.velD_change;
|
|
_output_vert_buffer[i].vel_d += _state_reset_status.velD_change;
|
|
}
|
|
}
|
|
|
|
// add the reset amount to the output observer vertical position state
|
|
if (vert_pos_reset) {
|
|
_output_vert_delayed.vel_d_integ = _state.pos(2);
|
|
_output_vert_new.vel_d_integ = _state.pos(2);
|
|
}
|
|
|
|
if (vert_vel_reset) {
|
|
_output_vert_delayed.vel_d = _state.vel(2);
|
|
_output_vert_new.vel_d = _state.vel(2);
|
|
}
|
|
}
|
|
|
|
// align output filter states to match EKF states at the fusion time horizon
|
|
void Ekf::alignOutputFilter()
|
|
{
|
|
// calculate the quaternion rotation delta from the EKF to output observer states at the EKF fusion time horizon
|
|
Quatf q_delta = _state.quat_nominal * _output_sample_delayed.quat_nominal.inversed();
|
|
q_delta.normalize();
|
|
|
|
// calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon
|
|
const Vector3f vel_delta = _state.vel - _output_sample_delayed.vel;
|
|
const Vector3f pos_delta = _state.pos - _output_sample_delayed.pos;
|
|
|
|
// loop through the output filter state history and add the deltas
|
|
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
|
|
_output_buffer[i].quat_nominal = q_delta * _output_buffer[i].quat_nominal;
|
|
_output_buffer[i].quat_nominal.normalize();
|
|
_output_buffer[i].vel += vel_delta;
|
|
_output_buffer[i].pos += pos_delta;
|
|
}
|
|
|
|
_output_new.quat_nominal = q_delta * _output_new.quat_nominal;
|
|
_output_new.quat_nominal.normalize();
|
|
|
|
_output_sample_delayed.quat_nominal = q_delta * _output_sample_delayed.quat_nominal;
|
|
_output_sample_delayed.quat_nominal.normalize();
|
|
}
|
|
|
|
// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
|
|
// It is used to align the yaw angle after launch or takeoff for fixed wing vehicle only.
|
|
bool Ekf::realignYawGPS()
|
|
{
|
|
// Need at least 5 m/s of GPS horizontal speed and ratio of velocity error to velocity < 0.15 for a reliable alignment
|
|
float gpsSpeed = sqrtf(sq(_gps_sample_delayed.vel(0)) + sq(_gps_sample_delayed.vel(1)));
|
|
|
|
if ((gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed))) {
|
|
// check for excessive horizontal GPS velocity innovations
|
|
bool badVelInnov = (_gps_vel_pos_test_ratio[HVEL] > 1.0f) && _control_status.flags.gps;
|
|
|
|
// calculate GPS course over ground angle
|
|
float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
|
|
|
|
// calculate course yaw angle
|
|
float ekfGOG = atan2f(_state.vel(1), _state.vel(0));
|
|
|
|
// Check the EKF and GPS course over ground for consistency
|
|
float courseYawError = gpsCOG - ekfGOG;
|
|
|
|
// If the angles disagree and horizontal GPS velocity innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
|
|
bool badYawErr = fabsf(courseYawError) > 0.5f;
|
|
bool badMagYaw = (badYawErr && badVelInnov);
|
|
|
|
if (badMagYaw) {
|
|
_num_bad_flight_yaw_events ++;
|
|
}
|
|
|
|
// correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned
|
|
if (badMagYaw || !_control_status.flags.yaw_align) {
|
|
ECL_WARN_TIMESTAMPED("EKF bad yaw corrected using GPS course");
|
|
|
|
// declare the magnetometer as failed if a bad yaw has occurred more than once
|
|
if (_control_status.flags.mag_aligned_in_flight && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) {
|
|
ECL_WARN_TIMESTAMPED("EKF stopping magnetometer use");
|
|
_control_status.flags.mag_fault = true;
|
|
}
|
|
|
|
// save a copy of the quaternion state for later use in calculating the amount of reset change
|
|
Quatf quat_before_reset = _state.quat_nominal;
|
|
|
|
// update transformation matrix from body to world frame using the current state estimate
|
|
_R_to_earth = Dcmf(_state.quat_nominal);
|
|
|
|
// get quaternion from existing filter states and calculate roll, pitch and yaw angles
|
|
Eulerf euler321(_state.quat_nominal);
|
|
|
|
// apply yaw correction
|
|
if (!_control_status.flags.mag_aligned_in_flight) {
|
|
// This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a
|
|
// forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error
|
|
euler321(2) += courseYawError;
|
|
_control_status.flags.mag_aligned_in_flight = true;
|
|
|
|
} else if (_control_status.flags.wind) {
|
|
// we have previously aligned yaw in-flight and have wind estimates so set the yaw such that the vehicle nose is
|
|
// aligned with the wind relative GPS velocity vector
|
|
euler321(2) = atan2f((_gps_sample_delayed.vel(1) - _state.wind_vel(1)),
|
|
(_gps_sample_delayed.vel(0) - _state.wind_vel(0)));
|
|
|
|
} else {
|
|
// we don't have wind estimates, so align yaw to the GPS velocity vector
|
|
euler321(2) = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
|
|
|
|
}
|
|
|
|
// calculate new filter quaternion states using corrected yaw angle
|
|
_state.quat_nominal = Quatf(euler321);
|
|
uncorrelateQuatStates();
|
|
|
|
// If heading was bad, then we also need to reset the velocity and position states
|
|
_velpos_reset_request = badMagYaw;
|
|
|
|
// update transformation matrix from body to world frame using the current state estimate
|
|
_R_to_earth = Dcmf(_state.quat_nominal);
|
|
|
|
// Use the last magnetometer measurements to reset the field states
|
|
_state.mag_B.zero();
|
|
_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
|
|
|
|
// use the combined EKF and GPS speed variance to calculate a rough estimate of the yaw error after alignment
|
|
float SpdErrorVariance = sq(_gps_sample_delayed.sacc) + P[4][4] + P[5][5];
|
|
float sineYawError = math::constrain(sqrtf(SpdErrorVariance) / gpsSpeed, 0.0f, 1.0f);
|
|
|
|
// adjust the quaternion covariances estimated yaw error
|
|
increaseQuatYawErrVariance(sq(asinf(sineYawError)));
|
|
|
|
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
|
|
clearMagCov();
|
|
|
|
if (_control_status.flags.mag_3D) {
|
|
for (uint8_t index = 16; index <= 21; index ++) {
|
|
P[index][index] = sq(_params.mag_noise);
|
|
}
|
|
|
|
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
|
|
saveMagCovData();
|
|
}
|
|
|
|
// record the start time for the magnetic field alignment
|
|
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
|
|
|
|
// calculate the amount that the quaternion has changed by
|
|
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
|
|
|
|
// add the reset amount to the output observer buffered data
|
|
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
|
|
_output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
|
|
}
|
|
|
|
// apply the change in attitude quaternion to our newest quaternion estimate
|
|
// which was already taken out from the output buffer
|
|
_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
|
|
|
|
// capture the reset event
|
|
_state_reset_status.quat_counter++;
|
|
|
|
return true;
|
|
|
|
} else {
|
|
// align mag states only
|
|
|
|
// calculate initial earth magnetic field states
|
|
_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
|
|
|
|
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
|
|
clearMagCov();
|
|
|
|
if (_control_status.flags.mag_3D) {
|
|
for (uint8_t index = 16; index <= 21; index ++) {
|
|
P[index][index] = sq(_params.mag_noise);
|
|
}
|
|
|
|
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
|
|
saveMagCovData();
|
|
}
|
|
|
|
// record the start time for the magnetic field alignment
|
|
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
|
|
|
|
return true;
|
|
}
|
|
|
|
} else {
|
|
// attempt a normal alignment using the magnetometer
|
|
return resetMagHeading(_mag_lpf.getState());
|
|
|
|
}
|
|
}
|
|
|
|
// Reset heading and magnetic field states
|
|
bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool update_buffer)
|
|
{
|
|
// prevent a reset being performed more than once on the same frame
|
|
if (_imu_sample_delayed.time_us == _flt_mag_align_start_time) {
|
|
return true;
|
|
}
|
|
|
|
if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE) {
|
|
stopMagFusion();
|
|
return false;
|
|
}
|
|
|
|
// save a copy of the quaternion state for later use in calculating the amount of reset change
|
|
Quatf quat_before_reset = _state.quat_nominal;
|
|
Quatf quat_after_reset = _state.quat_nominal;
|
|
|
|
// update transformation matrix from body to world frame using the current estimate
|
|
_R_to_earth = Dcmf(_state.quat_nominal);
|
|
|
|
// calculate the initial quaternion
|
|
// determine if a 321 or 312 Euler sequence is best
|
|
if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
|
|
// use a 321 sequence
|
|
|
|
// rotate the magnetometer measurement into earth frame
|
|
Eulerf euler321(_state.quat_nominal);
|
|
|
|
// Set the yaw angle to zero and calculate the rotation matrix from body to earth frame
|
|
euler321(2) = 0.0f;
|
|
Dcmf R_to_earth(euler321);
|
|
|
|
// calculate the observed yaw angle
|
|
if (_control_status.flags.ev_yaw) {
|
|
// convert the observed quaternion to a rotation matrix
|
|
Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
|
|
// calculate the yaw angle for a 312 sequence
|
|
euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0));
|
|
|
|
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
|
|
// rotate the magnetometer measurements into earth frame using a zero yaw angle
|
|
Vector3f mag_earth_pred = R_to_earth * mag_init;
|
|
// the angle of the projection onto the horizontal gives the yaw angle
|
|
euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
|
|
|
|
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) {
|
|
// we are operating without knowing the earth frame yaw angle
|
|
return true;
|
|
|
|
} else {
|
|
// there is no yaw observation
|
|
return false;
|
|
}
|
|
|
|
// calculate initial quaternion states for the ekf
|
|
// we don't change the output attitude to avoid jumps
|
|
quat_after_reset = Quatf(euler321);
|
|
|
|
} else {
|
|
// use a 312 sequence
|
|
|
|
// Calculate the 312 sequence euler angles that rotate from earth to body frame
|
|
// See http://www.atacolorado.com/eulersequences.doc
|
|
Vector3f euler312;
|
|
euler312(0) = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)); // first rotation (yaw)
|
|
euler312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
|
|
euler312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
|
|
|
|
// Set the first rotation (yaw) to zero and calculate the rotation matrix from body to earth frame
|
|
euler312(0) = 0.0f;
|
|
|
|
// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
|
|
float c2 = cosf(euler312(2));
|
|
float s2 = sinf(euler312(2));
|
|
float s1 = sinf(euler312(1));
|
|
float c1 = cosf(euler312(1));
|
|
float s0 = sinf(euler312(0));
|
|
float c0 = cosf(euler312(0));
|
|
|
|
Dcmf R_to_earth;
|
|
R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
|
|
R_to_earth(1, 1) = c0 * c1;
|
|
R_to_earth(2, 2) = c2 * c1;
|
|
R_to_earth(0, 1) = -c1 * s0;
|
|
R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
|
|
R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
|
|
R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
|
|
R_to_earth(2, 0) = -s2 * c1;
|
|
R_to_earth(2, 1) = s1;
|
|
|
|
// calculate the observed yaw angle
|
|
if (_control_status.flags.ev_yaw) {
|
|
// convert the observed quaternion to a rotation matrix
|
|
Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
|
|
// calculate the yaw angle for a 312 sequence
|
|
euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1));
|
|
|
|
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
|
|
// rotate the magnetometer measurements into earth frame using a zero yaw angle
|
|
Vector3f mag_earth_pred = R_to_earth * mag_init;
|
|
// the angle of the projection onto the horizontal gives the yaw angle
|
|
euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
|
|
|
|
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) {
|
|
// we are operating without knowing the earth frame yaw angle
|
|
return true;
|
|
|
|
} else {
|
|
// there is no yaw observation
|
|
return false;
|
|
}
|
|
|
|
// re-calculate the rotation matrix using the updated yaw angle
|
|
s0 = sinf(euler312(0));
|
|
c0 = cosf(euler312(0));
|
|
R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
|
|
R_to_earth(1, 1) = c0 * c1;
|
|
R_to_earth(2, 2) = c2 * c1;
|
|
R_to_earth(0, 1) = -c1 * s0;
|
|
R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
|
|
R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
|
|
R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
|
|
R_to_earth(2, 0) = -s2 * c1;
|
|
R_to_earth(2, 1) = s1;
|
|
|
|
// calculate initial quaternion states for the ekf
|
|
// we don't change the output attitude to avoid jumps
|
|
quat_after_reset = Quatf(R_to_earth);
|
|
}
|
|
|
|
// set the earth magnetic field states using the updated rotation
|
|
Dcmf R_to_earth_after(quat_after_reset);
|
|
_state.mag_I = R_to_earth_after * mag_init;
|
|
|
|
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
|
|
clearMagCov();
|
|
|
|
if (_control_status.flags.mag_3D) {
|
|
for (uint8_t index = 16; index <= 21; index ++) {
|
|
P[index][index] = sq(_params.mag_noise);
|
|
}
|
|
|
|
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
|
|
saveMagCovData();
|
|
}
|
|
|
|
// record the time for the magnetic field alignment event
|
|
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
|
|
|
|
// calculate the amount that the quaternion has changed by
|
|
Quatf q_error = quat_after_reset * quat_before_reset.inversed();
|
|
q_error.normalize();
|
|
|
|
// update quaternion states
|
|
_state.quat_nominal = quat_after_reset;
|
|
uncorrelateQuatStates();
|
|
|
|
// record the state change
|
|
_state_reset_status.quat_change = q_error;
|
|
|
|
// update transformation matrix from body to world frame using the current estimate
|
|
_R_to_earth = Dcmf(_state.quat_nominal);
|
|
|
|
// reset the rotation from the EV to EKF frame of reference if it is being used
|
|
if ((_params.fusion_mode & MASK_ROTATE_EV) && !_control_status.flags.ev_yaw) {
|
|
resetExtVisRotMat();
|
|
}
|
|
|
|
if (increase_yaw_var) {
|
|
// update the yaw angle variance using the variance of the measurement
|
|
if (_control_status.flags.ev_yaw) {
|
|
// using error estimate from external vision data
|
|
increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)));
|
|
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
|
|
// using magnetic heading tuning parameter
|
|
increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
|
|
}
|
|
}
|
|
|
|
if (update_buffer) {
|
|
// add the reset amount to the output observer buffered data
|
|
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
|
|
_output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
|
|
}
|
|
|
|
// apply the change in attitude quaternion to our newest quaternion estimate
|
|
// which was already taken out from the output buffer
|
|
_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
|
|
}
|
|
|
|
// capture the reset event
|
|
_state_reset_status.quat_counter++;
|
|
|
|
return true;
|
|
}
|
|
|
|
// Return the magnetic declination in radians to be used by the alignment and fusion processing
|
|
float Ekf::getMagDeclination()
|
|
{
|
|
// set source of magnetic declination for internal use
|
|
if (_control_status.flags.mag_aligned_in_flight) {
|
|
// Use value consistent with earth field state
|
|
return atan2f(_state.mag_I(1), _state.mag_I(0));
|
|
|
|
} else if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
|
|
// use parameter value until GPS is available, then use value returned by geo library
|
|
if (_NED_origin_initialised) {
|
|
return _mag_declination_gps;
|
|
|
|
} else {
|
|
return math::radians(_params.mag_declination_deg);
|
|
}
|
|
|
|
} else {
|
|
// always use the parameter value
|
|
return math::radians(_params.mag_declination_deg);
|
|
}
|
|
}
|
|
|
|
// This function forces the covariance matrix to be symmetric
|
|
void Ekf::makeSymmetrical(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
|
|
{
|
|
for (unsigned row = first; row <= last; row++) {
|
|
for (unsigned column = 0; column < row; column++) {
|
|
float tmp = (cov_mat[row][column] + cov_mat[column][row]) / 2;
|
|
cov_mat[row][column] = tmp;
|
|
cov_mat[column][row] = tmp;
|
|
}
|
|
}
|
|
}
|
|
|
|
void Ekf::constrainStates()
|
|
{
|
|
for (int i = 0; i < 4; i++) {
|
|
_state.quat_nominal(i) = math::constrain(_state.quat_nominal(i), -1.0f, 1.0f);
|
|
}
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
_state.vel(i) = math::constrain(_state.vel(i), -1000.0f, 1000.0f);
|
|
}
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
_state.pos(i) = math::constrain(_state.pos(i), -1.e6f, 1.e6f);
|
|
}
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
_state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -math::radians(20.f) * _dt_ekf_avg, math::radians(20.f) * _dt_ekf_avg);
|
|
}
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
_state.accel_bias(i) = math::constrain(_state.accel_bias(i), -_params.acc_bias_lim * _dt_ekf_avg, _params.acc_bias_lim * _dt_ekf_avg);
|
|
}
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
_state.mag_I(i) = math::constrain(_state.mag_I(i), -1.0f, 1.0f);
|
|
}
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
_state.mag_B(i) = math::constrain(_state.mag_B(i), -0.5f, 0.5f);
|
|
}
|
|
|
|
for (int i = 0; i < 2; i++) {
|
|
_state.wind_vel(i) = math::constrain(_state.wind_vel(i), -100.0f, 100.0f);
|
|
}
|
|
}
|
|
|
|
// calculate the earth rotation vector
|
|
void Ekf::calcEarthRateNED(Vector3f &omega, float lat_rad) const
|
|
{
|
|
omega(0) = CONSTANTS_EARTH_SPIN_RATE * cosf(lat_rad);
|
|
omega(1) = 0.0f;
|
|
omega(2) = -CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad);
|
|
}
|
|
|
|
void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos)
|
|
{
|
|
memcpy(hvel, _gps_vel_pos_innov+0, sizeof(float) * 2);
|
|
memcpy(&vvel, _gps_vel_pos_innov+2, sizeof(float) * 1);
|
|
memcpy(hpos, _gps_vel_pos_innov+3, sizeof(float) * 2);
|
|
memcpy(&vpos, _gps_vel_pos_innov+5, sizeof(float) * 1);
|
|
}
|
|
|
|
void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos)
|
|
{
|
|
memcpy(hvel, _gps_vel_pos_innov_var+0, sizeof(float) * 2);
|
|
memcpy(&vvel, _gps_vel_pos_innov_var+2, sizeof(float) * 1);
|
|
memcpy(hpos, _gps_vel_pos_innov_var+3, sizeof(float) * 2);
|
|
memcpy(&vpos, _gps_vel_pos_innov_var+5, sizeof(float) * 1);
|
|
}
|
|
|
|
void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos)
|
|
{
|
|
memcpy(&hvel, _gps_vel_pos_test_ratio+HVEL, sizeof(float));
|
|
memcpy(&vvel, _gps_vel_pos_test_ratio+VVEL, sizeof(float));
|
|
memcpy(&hpos, _gps_vel_pos_test_ratio+HPOS, sizeof(float));
|
|
memcpy(&vpos, _gps_vel_pos_test_ratio+VPOS, sizeof(float));
|
|
}
|
|
|
|
void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos)
|
|
{
|
|
memcpy(hvel, _ev_vel_pos_innov+0, sizeof(float) * 2);
|
|
memcpy(&vvel, _ev_vel_pos_innov+2, sizeof(float) * 1);
|
|
memcpy(hpos, _ev_vel_pos_innov+3, sizeof(float) * 2);
|
|
memcpy(&vpos, _ev_vel_pos_innov+5, sizeof(float) * 1);
|
|
}
|
|
|
|
void Ekf::getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos)
|
|
{
|
|
memcpy(hvel, _ev_vel_pos_innov_var+0, sizeof(float) * 2);
|
|
memcpy(&vvel, _ev_vel_pos_innov_var+2, sizeof(float) * 1);
|
|
memcpy(hpos, _ev_vel_pos_innov_var+3, sizeof(float) * 2);
|
|
memcpy(&vpos, _ev_vel_pos_innov_var+5, sizeof(float) * 1);
|
|
}
|
|
|
|
void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos)
|
|
{
|
|
memcpy(&hvel, _ev_vel_pos_test_ratio+HVEL, sizeof(float));
|
|
memcpy(&vvel, _ev_vel_pos_test_ratio+VVEL, sizeof(float));
|
|
memcpy(&hpos, _ev_vel_pos_test_ratio+HPOS, sizeof(float));
|
|
memcpy(&vpos, _ev_vel_pos_test_ratio+VPOS, sizeof(float));
|
|
}
|
|
|
|
void Ekf::getAuxVelInnov(float aux_vel_innov[2])
|
|
{
|
|
memcpy(aux_vel_innov, _aux_vel_innov, sizeof(_aux_vel_innov));
|
|
}
|
|
|
|
void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2])
|
|
{
|
|
memcpy(aux_vel_innov_var, _aux_vel_innov_var, sizeof(_aux_vel_innov_var));
|
|
}
|
|
|
|
void Ekf::getAuxVelInnovRatio(float &aux_vel_innov_ratio)
|
|
{
|
|
memcpy(&aux_vel_innov_ratio, &_aux_vel_test_ratio, sizeof(_aux_vel_test_ratio));
|
|
}
|
|
|
|
void Ekf::getFlowInnov(float flow_innov[2])
|
|
{
|
|
memcpy(flow_innov, _flow_innov, sizeof(_flow_innov));
|
|
}
|
|
|
|
void Ekf::getFlowInnovVar(float flow_innov_var[2])
|
|
{
|
|
memcpy(flow_innov_var, _flow_innov_var, sizeof(_flow_innov_var));
|
|
}
|
|
|
|
void Ekf::getFlowInnovRatio(float &flow_innov_ratio)
|
|
{
|
|
memcpy(&flow_innov_ratio, &_optflow_test_ratio, sizeof(_optflow_test_ratio));
|
|
}
|
|
|
|
void Ekf::getHeadingInnov(float &heading_innov)
|
|
{
|
|
memcpy(&heading_innov, &_heading_innov, sizeof(_heading_innov));
|
|
}
|
|
|
|
void Ekf::getHeadingInnovVar(float &heading_innov_var)
|
|
{
|
|
memcpy(&heading_innov_var, &_heading_innov_var, sizeof(_heading_innov_var));
|
|
}
|
|
|
|
void Ekf::getHeadingInnovRatio(float &heading_innov_ratio)
|
|
{
|
|
memcpy(&heading_innov_ratio, &_yaw_test_ratio, sizeof(_yaw_test_ratio));
|
|
}
|
|
|
|
void Ekf::getMagInnov(float mag_innov[3])
|
|
{
|
|
memcpy(mag_innov, _mag_innov, sizeof(_mag_innov));
|
|
}
|
|
|
|
void Ekf::getMagInnovVar(float mag_innov_var[3])
|
|
{
|
|
memcpy(mag_innov_var, _mag_innov_var, sizeof(_mag_innov_var));
|
|
}
|
|
|
|
void Ekf::getMagInnovRatio(float &mag_innov_ratio)
|
|
{
|
|
mag_innov_ratio = math::max(math::max(_mag_test_ratio[0], _mag_test_ratio[1]), _mag_test_ratio[2]);
|
|
}
|
|
|
|
void Ekf::getDragInnov(float drag_innov[2])
|
|
{
|
|
memcpy(&drag_innov, _drag_innov, sizeof(_drag_innov));
|
|
}
|
|
|
|
void Ekf::getDragInnovVar(float drag_innov_var[2])
|
|
{
|
|
memcpy(drag_innov_var, _drag_innov_var, sizeof(_drag_innov_var));
|
|
}
|
|
|
|
void Ekf::getDragInnovRatio(float drag_innov_ratio[2])
|
|
{
|
|
memcpy(drag_innov_ratio, &_drag_test_ratio, sizeof(_drag_test_ratio));
|
|
}
|
|
|
|
void Ekf::getAirspeedInnov(float &airspeed_innov)
|
|
{
|
|
memcpy(&airspeed_innov, &_airspeed_innov, sizeof(_airspeed_innov));
|
|
}
|
|
|
|
void Ekf::getAirspeedInnovVar(float &airspeed_innov_var)
|
|
{
|
|
memcpy(&airspeed_innov_var, &_airspeed_innov_var, sizeof(_airspeed_innov_var));
|
|
}
|
|
|
|
void Ekf::getAirspeedInnovRatio(float &airspeed_innov_ratio)
|
|
{
|
|
memcpy(&airspeed_innov_ratio, &_tas_test_ratio, sizeof(_tas_test_ratio));
|
|
}
|
|
|
|
void Ekf::getBetaInnov(float &beta_innov)
|
|
{
|
|
memcpy(&beta_innov, &_beta_innov, sizeof(_beta_innov));
|
|
}
|
|
|
|
void Ekf::getBetaInnovVar(float &beta_innov_var)
|
|
{
|
|
memcpy(&beta_innov_var, &_beta_innov_var, sizeof(_beta_innov_var));
|
|
}
|
|
|
|
void Ekf::getBetaInnovRatio(float &beta_innov_ratio)
|
|
{
|
|
memcpy(&beta_innov_ratio, &_beta_test_ratio, sizeof(_beta_test_ratio));
|
|
}
|
|
|
|
void Ekf::getHaglInnov(float &hagl_innov)
|
|
{
|
|
memcpy(&hagl_innov, &_hagl_innov, sizeof(_hagl_innov));
|
|
}
|
|
|
|
void Ekf::getHaglInnovVar(float &hagl_innov_var)
|
|
{
|
|
memcpy(&hagl_innov_var, &_hagl_innov_var, sizeof(_hagl_innov_var));
|
|
}
|
|
|
|
void Ekf::getHaglInnovRatio(float &hagl_innov_ratio)
|
|
{
|
|
memcpy(&hagl_innov_ratio, &_hagl_test_ratio, sizeof(_hagl_test_ratio));
|
|
}
|
|
|
|
// get GPS check status
|
|
void Ekf::get_gps_check_status(uint16_t *val)
|
|
{
|
|
*val = _gps_check_fail_status.value;
|
|
}
|
|
|
|
// get the state vector at the delayed time horizon
|
|
void Ekf::get_state_delayed(float *state)
|
|
{
|
|
for (int i = 0; i < 4; i++) {
|
|
state[i] = _state.quat_nominal(i);
|
|
}
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
state[i + 4] = _state.vel(i);
|
|
}
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
state[i + 7] = _state.pos(i);
|
|
}
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
state[i + 10] = _state.gyro_bias(i);
|
|
}
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
state[i + 13] = _state.accel_bias(i);
|
|
}
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
state[i + 16] = _state.mag_I(i);
|
|
}
|
|
|
|
for (int i = 0; i < 3; i++) {
|
|
state[i + 19] = _state.mag_B(i);
|
|
}
|
|
|
|
for (int i = 0; i < 2; i++) {
|
|
state[i + 22] = _state.wind_vel(i);
|
|
}
|
|
}
|
|
|
|
// get the accelerometer bias
|
|
void Ekf::get_accel_bias(float bias[3])
|
|
{
|
|
float temp[3];
|
|
temp[0] = _state.accel_bias(0) / _dt_ekf_avg;
|
|
temp[1] = _state.accel_bias(1) / _dt_ekf_avg;
|
|
temp[2] = _state.accel_bias(2) / _dt_ekf_avg;
|
|
memcpy(bias, temp, 3 * sizeof(float));
|
|
}
|
|
|
|
// get the gyroscope bias in rad/s
|
|
void Ekf::get_gyro_bias(float bias[3])
|
|
{
|
|
float temp[3];
|
|
temp[0] = _state.gyro_bias(0) / _dt_ekf_avg;
|
|
temp[1] = _state.gyro_bias(1) / _dt_ekf_avg;
|
|
temp[2] = _state.gyro_bias(2) / _dt_ekf_avg;
|
|
memcpy(bias, temp, 3 * sizeof(float));
|
|
}
|
|
|
|
// get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set
|
|
// return true if the origin is valid
|
|
bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt)
|
|
{
|
|
memcpy(origin_time, &_last_gps_origin_time_us, sizeof(uint64_t));
|
|
memcpy(origin_pos, &_pos_ref, sizeof(map_projection_reference_s));
|
|
memcpy(origin_alt, &_gps_alt_ref, sizeof(float));
|
|
return _NED_origin_initialised;
|
|
}
|
|
|
|
// return an array containing the output predictor angular, velocity and position tracking
|
|
// error magnitudes (rad), (m/s), (m)
|
|
void Ekf::get_output_tracking_error(float error[3])
|
|
{
|
|
memcpy(error, _output_tracking_error, 3 * sizeof(float));
|
|
}
|
|
|
|
/*
|
|
Returns following IMU vibration metrics in the following array locations
|
|
0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
|
|
1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
|
|
2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
|
|
*/
|
|
void Ekf::get_imu_vibe_metrics(float vibe[3])
|
|
{
|
|
memcpy(vibe, _vibe_metrics, 3 * sizeof(float));
|
|
}
|
|
|
|
/*
|
|
First argument returns GPS drift metrics in the following array locations
|
|
0 : Horizontal position drift rate (m/s)
|
|
1 : Vertical position drift rate (m/s)
|
|
2 : Filtered horizontal velocity (m/s)
|
|
Second argument returns true when IMU movement is blocking the drift calculation
|
|
Function returns true if the metrics have been updated and not returned previously by this function
|
|
*/
|
|
bool Ekf::get_gps_drift_metrics(float drift[3], bool *blocked)
|
|
{
|
|
memcpy(drift, _gps_drift_metrics, 3 * sizeof(float));
|
|
*blocked = !_vehicle_at_rest;
|
|
if (_gps_drift_updated) {
|
|
_gps_drift_updated = false;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
|
|
void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv)
|
|
{
|
|
// report absolute accuracy taking into account the uncertainty in location of the origin
|
|
// If not aiding, return 0 for horizontal position estimate as no estimate is available
|
|
// TODO - allow for baro drift in vertical position error
|
|
float hpos_err = sqrtf(P[7][7] + P[8][8] + sq(_gps_origin_eph));
|
|
|
|
// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error
|
|
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
|
|
// and using state variances for accuracy reporting is overly optimistic in these situations
|
|
if (_is_dead_reckoning && (_control_status.flags.gps || _control_status.flags.ev_pos)) {
|
|
hpos_err = math::max(hpos_err, sqrtf(sq(_vel_pos_innov[3]) + sq(_vel_pos_innov[4])));
|
|
}
|
|
|
|
*ekf_eph = hpos_err;
|
|
*ekf_epv = sqrtf(P[9][9] + sq(_gps_origin_epv));
|
|
}
|
|
|
|
// get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
|
|
void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv)
|
|
{
|
|
// TODO - allow for baro drift in vertical position error
|
|
float hpos_err = sqrtf(P[7][7] + P[8][8]);
|
|
|
|
// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error
|
|
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
|
|
// and using state variances for accuracy reporting is overly optimistic in these situations
|
|
if (_is_dead_reckoning && (_control_status.flags.gps || _control_status.flags.ev_pos)) {
|
|
hpos_err = math::max(hpos_err, sqrtf(sq(_vel_pos_innov[3]) + sq(_vel_pos_innov[4])));
|
|
}
|
|
|
|
*ekf_eph = hpos_err;
|
|
*ekf_epv = sqrtf(P[9][9]);
|
|
}
|
|
|
|
// get the 1-sigma horizontal and vertical velocity uncertainty
|
|
void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv)
|
|
{
|
|
float hvel_err = sqrtf(P[4][4] + P[5][5]);
|
|
|
|
// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal velocity error
|
|
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
|
|
// and using state variances for accuracy reporting is overly optimistic in these situations
|
|
if (_is_dead_reckoning) {
|
|
float vel_err_conservative = 0.0f;
|
|
|
|
if (_control_status.flags.opt_flow) {
|
|
float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
|
|
vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * sqrtf(sq(_flow_innov[0]) + sq(_flow_innov[1]));
|
|
}
|
|
|
|
if (_control_status.flags.gps || _control_status.flags.ev_pos) {
|
|
vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_vel_pos_innov[0]) + sq(_vel_pos_innov[1])));
|
|
}
|
|
|
|
if (_control_status.flags.ev_vel) {
|
|
// What is the right thing to do here
|
|
// vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_vel_pos_innov[0]) + sq(_vel_pos_innov[1])));
|
|
}
|
|
|
|
hvel_err = math::max(hvel_err, vel_err_conservative);
|
|
}
|
|
|
|
*ekf_evh = hvel_err;
|
|
*ekf_evv = sqrtf(P[6][6]);
|
|
}
|
|
|
|
/*
|
|
Returns the following vehicle control limits required by the estimator to keep within sensor limitations.
|
|
vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed.
|
|
vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed.
|
|
hagl_min : Minimum height above ground (meters). NaN when limiting is not needed.
|
|
hagl_max : Maximum height above ground (meters). NaN when limiting is not needed.
|
|
*/
|
|
void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max)
|
|
{
|
|
// Calculate range finder limits
|
|
float rangefinder_hagl_min = _rng_valid_min_val;
|
|
// Allow use of 75% of rangefinder maximum range to allow for angular motion
|
|
float rangefinder_hagl_max = 0.75f * _rng_valid_max_val;
|
|
|
|
// Calculate optical flow limits
|
|
// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
|
|
float flow_vxy_max = fmaxf(0.5f * _flow_max_rate * (_terrain_vpos - _state.pos(2)), 0.0f);
|
|
float flow_hagl_min = _flow_min_distance;
|
|
float flow_hagl_max = _flow_max_distance;
|
|
|
|
// TODO : calculate visual odometry limits
|
|
|
|
bool relying_on_rangefinder = _control_status.flags.rng_hgt && !_params.range_aid;
|
|
|
|
bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel);
|
|
|
|
// Do not require limiting by default
|
|
*vxy_max = NAN;
|
|
*vz_max = NAN;
|
|
*hagl_min = NAN;
|
|
*hagl_max = NAN;
|
|
|
|
// Keep within range sensor limit when using rangefinder as primary height source
|
|
if (relying_on_rangefinder) {
|
|
*vxy_max = NAN;
|
|
*vz_max = NAN;
|
|
*hagl_min = rangefinder_hagl_min;
|
|
*hagl_max = rangefinder_hagl_max;
|
|
}
|
|
|
|
// Keep within flow AND range sensor limits when exclusively using optical flow
|
|
if (relying_on_optical_flow) {
|
|
*vxy_max = flow_vxy_max;
|
|
*vz_max = NAN;
|
|
*hagl_min = fmaxf(rangefinder_hagl_min, flow_hagl_min);
|
|
*hagl_max = fminf(rangefinder_hagl_max, flow_hagl_max);
|
|
}
|
|
|
|
}
|
|
|
|
bool Ekf::reset_imu_bias()
|
|
{
|
|
if (_imu_sample_delayed.time_us - _last_imu_bias_cov_reset_us < (uint64_t)10e6) {
|
|
return false;
|
|
|
|
}
|
|
|
|
// Zero the delta angle and delta velocity bias states
|
|
_state.gyro_bias.zero();
|
|
_state.accel_bias.zero();
|
|
|
|
// Zero the corresponding covariances
|
|
zeroCols(P, 10, 15);
|
|
zeroRows(P, 10, 15);
|
|
|
|
// Set the corresponding variances to the values use for initial alignment
|
|
float dt = FILTER_UPDATE_PERIOD_S;
|
|
P[12][12] = P[11][11] = P[10][10] = sq(_params.switch_on_gyro_bias * dt);
|
|
P[15][15] = P[14][14] = P[13][13] = sq(_params.switch_on_accel_bias * dt);
|
|
_last_imu_bias_cov_reset_us = _imu_sample_delayed.time_us;
|
|
|
|
// Set previous frame values
|
|
_prev_dvel_bias_var(0) = P[13][13];
|
|
_prev_dvel_bias_var(1) = P[14][14];
|
|
_prev_dvel_bias_var(2) = P[15][15];
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
// get EKF innovation consistency check status information comprising of:
|
|
// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
|
|
// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
|
|
// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
|
|
// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
|
|
void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &gps_vel, float &gps_pos, float &ev_vel, float &ev_pos, float &hgt, float &tas, float &hagl, float &beta)
|
|
{
|
|
// return the integer bitmask containing the consistency check pass/fail status
|
|
status = _innov_check_fail_status.value;
|
|
// return the largest magnetometer innovation test ratio
|
|
mag = sqrtf(math::max(_yaw_test_ratio, math::max(math::max(_mag_test_ratio[0], _mag_test_ratio[1]), _mag_test_ratio[2])));
|
|
// return the largest NED GPS velocity innovation test ratio
|
|
gps_vel = sqrtf(math::max(_gps_vel_pos_test_ratio[HVEL], _gps_vel_pos_test_ratio[VVEL]));
|
|
// return the largest NE GPS position innovation test ratio
|
|
gps_pos = sqrtf(_gps_vel_pos_test_ratio[HPOS]);
|
|
// return the largest external vision velocity innovation test ratio
|
|
ev_vel = sqrtf(math::max(_ev_vel_pos_test_ratio[HVEL], _ev_vel_pos_test_ratio[VVEL]));
|
|
// return the largest horizontal external vision position innovation test ratio
|
|
ev_pos = sqrtf(_ev_vel_pos_test_ratio[HPOS]);
|
|
// return the vertical position innovation test ratio
|
|
hgt = sqrtf(_gps_vel_pos_test_ratio[VPOS]);
|
|
// return the airspeed fusion innovation test ratio
|
|
tas = sqrtf(_tas_test_ratio);
|
|
// return the terrain height innovation test ratio
|
|
hagl = sqrtf(_hagl_test_ratio);
|
|
// return the synthetic sideslip innovation test ratio
|
|
beta = sqrtf(_beta_test_ratio);
|
|
}
|
|
|
|
// return a bitmask integer that describes which state estimates are valid
|
|
void Ekf::get_ekf_soln_status(uint16_t *status)
|
|
{
|
|
ekf_solution_status soln_status;
|
|
|
|
soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0);
|
|
soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos|| _control_status.flags.ev_vel || _control_status.flags.opt_flow || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
|
|
soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0);
|
|
soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
|
|
soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0);
|
|
soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
|
|
soln_status.flags.pos_vert_agl = isTerrainEstimateValid();
|
|
soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
|
|
soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
|
|
soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
|
|
bool gps_vel_innov_bad = (_gps_vel_pos_test_ratio[HVEL] > 1.0f) || (_gps_vel_pos_test_ratio[VVEL] > 1.0f);
|
|
bool gps_pos_innov_bad = (_gps_vel_pos_test_ratio[HPOS] > 1.0f);
|
|
bool mag_innov_good = (_mag_test_ratio[0] < 1.0f) && (_mag_test_ratio[1] < 1.0f) && (_mag_test_ratio[2] < 1.0f) && (_yaw_test_ratio < 1.0f);
|
|
soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
|
|
soln_status.flags.accel_error = _bad_vert_accel_detected;
|
|
*status = soln_status.value;
|
|
}
|
|
|
|
// fuse measurement
|
|
void Ekf::fuse(float *K, float innovation)
|
|
{
|
|
for (unsigned i = 0; i < 4; i++) {
|
|
_state.quat_nominal(i) = _state.quat_nominal(i) - K[i] * innovation;
|
|
}
|
|
|
|
_state.quat_nominal.normalize();
|
|
|
|
for (unsigned i = 0; i < 3; i++) {
|
|
_state.vel(i) = _state.vel(i) - K[i + 4] * innovation;
|
|
}
|
|
|
|
for (unsigned i = 0; i < 3; i++) {
|
|
_state.pos(i) = _state.pos(i) - K[i + 7] * innovation;
|
|
}
|
|
|
|
for (unsigned i = 0; i < 3; i++) {
|
|
_state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 10] * innovation;
|
|
}
|
|
|
|
for (unsigned i = 0; i < 3; i++) {
|
|
_state.accel_bias(i) = _state.accel_bias(i) - K[i + 13] * innovation;
|
|
}
|
|
|
|
for (unsigned i = 0; i < 3; i++) {
|
|
_state.mag_I(i) = _state.mag_I(i) - K[i + 16] * innovation;
|
|
}
|
|
|
|
for (unsigned i = 0; i < 3; i++) {
|
|
_state.mag_B(i) = _state.mag_B(i) - K[i + 19] * innovation;
|
|
}
|
|
|
|
for (unsigned i = 0; i < 2; i++) {
|
|
_state.wind_vel(i) = _state.wind_vel(i) - K[i + 22] * innovation;
|
|
}
|
|
}
|
|
|
|
// zero specified range of rows in the state covariance matrix
|
|
void Ekf::zeroRows(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
|
|
{
|
|
uint8_t row;
|
|
|
|
for (row = first; row <= last; row++) {
|
|
memset(&cov_mat[row][0], 0, sizeof(cov_mat[0][0]) * 24);
|
|
}
|
|
}
|
|
|
|
// zero specified range of columns in the state covariance matrix
|
|
void Ekf::zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
|
|
{
|
|
uint8_t row;
|
|
|
|
for (row = 0; row <= 23; row++) {
|
|
memset(&cov_mat[row][first], 0, sizeof(cov_mat[0][0]) * (1 + last - first));
|
|
}
|
|
}
|
|
|
|
void Ekf::zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
|
|
{
|
|
// save diagonal elements
|
|
uint8_t row;
|
|
float variances[_k_num_states];
|
|
|
|
for (row = first; row <= last; row++) {
|
|
variances[row] = cov_mat[row][row];
|
|
}
|
|
|
|
// zero rows and columns
|
|
zeroRows(cov_mat, first, last);
|
|
zeroCols(cov_mat, first, last);
|
|
|
|
// restore diagonals
|
|
for (row = first; row <= last; row++) {
|
|
cov_mat[row][row] = variances[row];
|
|
}
|
|
}
|
|
|
|
void Ekf::uncorrelateQuatStates()
|
|
{
|
|
// save 4x4 elements
|
|
uint32_t row;
|
|
uint32_t col;
|
|
float variances[4][4];
|
|
for (row = 0; row < 4; row++) {
|
|
for (col = 0; col < 4; col++) {
|
|
variances[row][col] = P[row][col];
|
|
}
|
|
}
|
|
|
|
// zero rows and columns
|
|
zeroRows(P, 0, 3);
|
|
zeroCols(P, 0, 3);
|
|
|
|
// restore 4x4 elements
|
|
for (row = 0; row < 4; row++) {
|
|
for (col = 0; col < 4; col++) {
|
|
P[row][col] = variances[row][col];
|
|
}
|
|
}
|
|
}
|
|
|
|
void Ekf::setDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last, float variance)
|
|
{
|
|
// zero rows and columns
|
|
zeroRows(cov_mat, first, last);
|
|
zeroCols(cov_mat, first, last);
|
|
|
|
// set diagonals
|
|
uint8_t row;
|
|
|
|
for (row = first; row <= last; row++) {
|
|
cov_mat[row][row] = variance;
|
|
}
|
|
|
|
}
|
|
|
|
bool Ekf::global_position_is_valid()
|
|
{
|
|
// return true if the origin is set we are not doing unconstrained free inertial navigation
|
|
// and have not started using synthetic position observations to constrain drift
|
|
return (_NED_origin_initialised && !_deadreckon_time_exceeded && !_using_synthetic_position);
|
|
}
|
|
|
|
// return true if we are totally reliant on inertial dead-reckoning for position
|
|
void Ekf::update_deadreckoning_status()
|
|
{
|
|
bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel)
|
|
&& (((_time_last_imu - _time_last_pos_fuse) <= _params.no_aid_timeout_max)
|
|
|| ((_time_last_imu - _time_last_vel_fuse) <= _params.no_aid_timeout_max)
|
|
|| ((_time_last_imu - _time_last_delpos_fuse) <= _params.no_aid_timeout_max));
|
|
bool optFlowAiding = _control_status.flags.opt_flow && ((_time_last_imu - _time_last_of_fuse) <= _params.no_aid_timeout_max);
|
|
bool airDataAiding = _control_status.flags.wind && ((_time_last_imu - _time_last_arsp_fuse) <= _params.no_aid_timeout_max) && ((_time_last_imu - _time_last_beta_fuse) <= _params.no_aid_timeout_max);
|
|
|
|
_is_wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding;
|
|
_is_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
|
|
|
|
// record the time we start inertial dead reckoning
|
|
if (!_is_dead_reckoning) {
|
|
_time_ins_deadreckon_start = _time_last_imu - _params.no_aid_timeout_max;
|
|
}
|
|
|
|
// report if we have been deadreckoning for too long
|
|
_deadreckon_time_exceeded = ((_time_last_imu - _time_ins_deadreckon_start) > (unsigned)_params.valid_timeout_max);
|
|
}
|
|
|
|
// perform a vector cross product
|
|
Vector3f EstimatorInterface::cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2)
|
|
{
|
|
Vector3f vecOut;
|
|
vecOut(0) = vecIn1(1) * vecIn2(2) - vecIn1(2) * vecIn2(1);
|
|
vecOut(1) = vecIn1(2) * vecIn2(0) - vecIn1(0) * vecIn2(2);
|
|
vecOut(2) = vecIn1(0) * vecIn2(1) - vecIn1(1) * vecIn2(0);
|
|
return vecOut;
|
|
}
|
|
|
|
// calculate the inverse rotation matrix from a quaternion rotation
|
|
// this produces the inverse rotation to that produced by the math library quaternion to Dcmf operator
|
|
Matrix3f EstimatorInterface::quat_to_invrotmat(const Quatf &quat)
|
|
{
|
|
float q00 = quat(0) * quat(0);
|
|
float q11 = quat(1) * quat(1);
|
|
float q22 = quat(2) * quat(2);
|
|
float q33 = quat(3) * quat(3);
|
|
float q01 = quat(0) * quat(1);
|
|
float q02 = quat(0) * quat(2);
|
|
float q03 = quat(0) * quat(3);
|
|
float q12 = quat(1) * quat(2);
|
|
float q13 = quat(1) * quat(3);
|
|
float q23 = quat(2) * quat(3);
|
|
|
|
Matrix3f dcm;
|
|
dcm(0, 0) = q00 + q11 - q22 - q33;
|
|
dcm(1, 1) = q00 - q11 + q22 - q33;
|
|
dcm(2, 2) = q00 - q11 - q22 + q33;
|
|
dcm(1, 0) = 2.0f * (q12 - q03);
|
|
dcm(2, 0) = 2.0f * (q13 + q02);
|
|
dcm(0, 1) = 2.0f * (q12 + q03);
|
|
dcm(2, 1) = 2.0f * (q23 - q01);
|
|
dcm(0, 2) = 2.0f * (q13 - q02);
|
|
dcm(1, 2) = 2.0f * (q23 + q01);
|
|
|
|
return dcm;
|
|
}
|
|
|
|
// calculate the variances for the rotation vector equivalent
|
|
Vector3f Ekf::calcRotVecVariances()
|
|
{
|
|
Vector3f rot_var_vec;
|
|
float q0, q1, q2, q3;
|
|
|
|
if (_state.quat_nominal(0) >= 0.0f) {
|
|
q0 = _state.quat_nominal(0);
|
|
q1 = _state.quat_nominal(1);
|
|
q2 = _state.quat_nominal(2);
|
|
q3 = _state.quat_nominal(3);
|
|
|
|
} else {
|
|
q0 = -_state.quat_nominal(0);
|
|
q1 = -_state.quat_nominal(1);
|
|
q2 = -_state.quat_nominal(2);
|
|
q3 = -_state.quat_nominal(3);
|
|
}
|
|
float t2 = q0*q0;
|
|
float t3 = acosf(q0);
|
|
float t4 = -t2+1.0f;
|
|
float t5 = t2-1.0f;
|
|
if ((t4 > 1e-9f) && (t5 < -1e-9f)) {
|
|
float t6 = 1.0f/t5;
|
|
float t7 = q1*t6*2.0f;
|
|
float t8 = 1.0f/powf(t4,1.5f);
|
|
float t9 = q0*q1*t3*t8*2.0f;
|
|
float t10 = t7+t9;
|
|
float t11 = 1.0f/sqrtf(t4);
|
|
float t12 = q2*t6*2.0f;
|
|
float t13 = q0*q2*t3*t8*2.0f;
|
|
float t14 = t12+t13;
|
|
float t15 = q3*t6*2.0f;
|
|
float t16 = q0*q3*t3*t8*2.0f;
|
|
float t17 = t15+t16;
|
|
rot_var_vec(0) = t10*(P[0][0]*t10+P[1][0]*t3*t11*2.0f)+t3*t11*(P[0][1]*t10+P[1][1]*t3*t11*2.0f)*2.0f;
|
|
rot_var_vec(1) = t14*(P[0][0]*t14+P[2][0]*t3*t11*2.0f)+t3*t11*(P[0][2]*t14+P[2][2]*t3*t11*2.0f)*2.0f;
|
|
rot_var_vec(2) = t17*(P[0][0]*t17+P[3][0]*t3*t11*2.0f)+t3*t11*(P[0][3]*t17+P[3][3]*t3*t11*2.0f)*2.0f;
|
|
} else {
|
|
rot_var_vec(0) = 4.0f * P[1][1];
|
|
rot_var_vec(1) = 4.0f * P[2][2];
|
|
rot_var_vec(2) = 4.0f * P[3][3];
|
|
}
|
|
|
|
return rot_var_vec;
|
|
}
|
|
|
|
// initialise the quaternion covariances using rotation vector variances
|
|
void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
|
|
{
|
|
// calculate an equivalent rotation vector from the quaternion
|
|
float q0,q1,q2,q3;
|
|
if (_state.quat_nominal(0) >= 0.0f) {
|
|
q0 = _state.quat_nominal(0);
|
|
q1 = _state.quat_nominal(1);
|
|
q2 = _state.quat_nominal(2);
|
|
q3 = _state.quat_nominal(3);
|
|
|
|
} else {
|
|
q0 = -_state.quat_nominal(0);
|
|
q1 = -_state.quat_nominal(1);
|
|
q2 = -_state.quat_nominal(2);
|
|
q3 = -_state.quat_nominal(3);
|
|
}
|
|
float delta = 2.0f*acosf(q0);
|
|
float scaler = (delta/sinf(delta*0.5f));
|
|
float rotX = scaler*q1;
|
|
float rotY = scaler*q2;
|
|
float rotZ = scaler*q3;
|
|
|
|
// autocode generated using matlab symbolic toolbox
|
|
float t2 = rotX*rotX;
|
|
float t4 = rotY*rotY;
|
|
float t5 = rotZ*rotZ;
|
|
float t6 = t2+t4+t5;
|
|
if (t6 > 1e-9f) {
|
|
float t7 = sqrtf(t6);
|
|
float t8 = t7*0.5f;
|
|
float t3 = sinf(t8);
|
|
float t9 = t3*t3;
|
|
float t10 = 1.0f/t6;
|
|
float t11 = 1.0f/sqrtf(t6);
|
|
float t12 = cosf(t8);
|
|
float t13 = 1.0f/powf(t6,1.5f);
|
|
float t14 = t3*t11;
|
|
float t15 = rotX*rotY*t3*t13;
|
|
float t16 = rotX*rotZ*t3*t13;
|
|
float t17 = rotY*rotZ*t3*t13;
|
|
float t18 = t2*t10*t12*0.5f;
|
|
float t27 = t2*t3*t13;
|
|
float t19 = t14+t18-t27;
|
|
float t23 = rotX*rotY*t10*t12*0.5f;
|
|
float t28 = t15-t23;
|
|
float t20 = rotY*rot_vec_var(1)*t3*t11*t28*0.5f;
|
|
float t25 = rotX*rotZ*t10*t12*0.5f;
|
|
float t31 = t16-t25;
|
|
float t21 = rotZ*rot_vec_var(2)*t3*t11*t31*0.5f;
|
|
float t22 = t20+t21-rotX*rot_vec_var(0)*t3*t11*t19*0.5f;
|
|
float t24 = t15-t23;
|
|
float t26 = t16-t25;
|
|
float t29 = t4*t10*t12*0.5f;
|
|
float t34 = t3*t4*t13;
|
|
float t30 = t14+t29-t34;
|
|
float t32 = t5*t10*t12*0.5f;
|
|
float t40 = t3*t5*t13;
|
|
float t33 = t14+t32-t40;
|
|
float t36 = rotY*rotZ*t10*t12*0.5f;
|
|
float t39 = t17-t36;
|
|
float t35 = rotZ*rot_vec_var(2)*t3*t11*t39*0.5f;
|
|
float t37 = t15-t23;
|
|
float t38 = t17-t36;
|
|
float t41 = rot_vec_var(0)*(t15-t23)*(t16-t25);
|
|
float t42 = t41-rot_vec_var(1)*t30*t39-rot_vec_var(2)*t33*t39;
|
|
float t43 = t16-t25;
|
|
float t44 = t17-t36;
|
|
|
|
// zero all the quaternion covariances
|
|
zeroRows(P, 0, 3);
|
|
zeroCols(P, 0, 3);
|
|
|
|
// Update the quaternion internal covariances using auto-code generated using matlab symbolic toolbox
|
|
P[0][0] = rot_vec_var(0)*t2*t9*t10*0.25f+rot_vec_var(1)*t4*t9*t10*0.25f+rot_vec_var(2)*t5*t9*t10*0.25f;
|
|
P[0][1] = t22;
|
|
P[0][2] = t35+rotX*rot_vec_var(0)*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rot_vec_var(1)*t3*t11*t30*0.5f;
|
|
P[0][3] = rotX*rot_vec_var(0)*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rot_vec_var(2)*t3*t11*t33*0.5f;
|
|
P[1][0] = t22;
|
|
P[1][1] = rot_vec_var(0)*(t19*t19)+rot_vec_var(1)*(t24*t24)+rot_vec_var(2)*(t26*t26);
|
|
P[1][2] = rot_vec_var(2)*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30;
|
|
P[1][3] = rot_vec_var(1)*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33;
|
|
P[2][0] = t35-rotY*rot_vec_var(1)*t3*t11*t30*0.5f+rotX*rot_vec_var(0)*t3*t11*(t15-t23)*0.5f;
|
|
P[2][1] = rot_vec_var(2)*(t16-t25)*(t17-t36)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30;
|
|
P[2][2] = rot_vec_var(1)*(t30*t30)+rot_vec_var(0)*(t37*t37)+rot_vec_var(2)*(t38*t38);
|
|
P[2][3] = t42;
|
|
P[3][0] = rotZ*rot_vec_var(2)*t3*t11*t33*(-0.5f)+rotX*rot_vec_var(0)*t3*t11*(t16-t25)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-t36)*0.5f;
|
|
P[3][1] = rot_vec_var(1)*(t15-t23)*(t17-t36)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33;
|
|
P[3][2] = t42;
|
|
P[3][3] = rot_vec_var(2)*(t33*t33)+rot_vec_var(0)*(t43*t43)+rot_vec_var(1)*(t44*t44);
|
|
|
|
} else {
|
|
// the equations are badly conditioned so use a small angle approximation
|
|
P[0][0] = 0.0f;
|
|
P[0][1] = 0.0f;
|
|
P[0][2] = 0.0f;
|
|
P[0][3] = 0.0f;
|
|
P[1][0] = 0.0f;
|
|
P[1][1] = 0.25f * rot_vec_var(0);
|
|
P[1][2] = 0.0f;
|
|
P[1][3] = 0.0f;
|
|
P[2][0] = 0.0f;
|
|
P[2][1] = 0.0f;
|
|
P[2][2] = 0.25f * rot_vec_var(1);
|
|
P[2][3] = 0.0f;
|
|
P[3][0] = 0.0f;
|
|
P[3][1] = 0.0f;
|
|
P[3][2] = 0.0f;
|
|
P[3][3] = 0.25f * rot_vec_var(2);
|
|
|
|
}
|
|
}
|
|
|
|
void Ekf::setControlBaroHeight()
|
|
{
|
|
_control_status.flags.baro_hgt = true;
|
|
|
|
_control_status.flags.gps_hgt = false;
|
|
_control_status.flags.rng_hgt = false;
|
|
_control_status.flags.ev_hgt = false;
|
|
}
|
|
|
|
void Ekf::setControlRangeHeight()
|
|
{
|
|
_control_status.flags.rng_hgt = true;
|
|
|
|
_control_status.flags.baro_hgt = false;
|
|
_control_status.flags.gps_hgt = false;
|
|
_control_status.flags.ev_hgt = false;
|
|
}
|
|
|
|
void Ekf::setControlGPSHeight()
|
|
{
|
|
_control_status.flags.gps_hgt = true;
|
|
|
|
_control_status.flags.baro_hgt = false;
|
|
_control_status.flags.rng_hgt = false;
|
|
_control_status.flags.ev_hgt = false;
|
|
}
|
|
|
|
void Ekf::setControlEVHeight()
|
|
{
|
|
_control_status.flags.ev_hgt = true;
|
|
|
|
_control_status.flags.baro_hgt = false;
|
|
_control_status.flags.gps_hgt = false;
|
|
_control_status.flags.rng_hgt = false;
|
|
}
|
|
|
|
void Ekf::stopMagFusion()
|
|
{
|
|
stopMag3DFusion();
|
|
stopMagHdgFusion();
|
|
clearMagCov();
|
|
}
|
|
|
|
void Ekf::stopMag3DFusion()
|
|
{
|
|
// save covariance data for re-use if currently doing 3-axis fusion
|
|
if (_control_status.flags.mag_3D) {
|
|
saveMagCovData();
|
|
_control_status.flags.mag_3D = false;
|
|
}
|
|
}
|
|
|
|
void Ekf::stopMagHdgFusion()
|
|
{
|
|
_control_status.flags.mag_hdg = false;
|
|
}
|
|
|
|
void Ekf::startMagHdgFusion()
|
|
{
|
|
stopMag3DFusion();
|
|
_control_status.flags.mag_hdg = true;
|
|
}
|
|
|
|
void Ekf::startMag3DFusion()
|
|
{
|
|
if (!_control_status.flags.mag_3D) {
|
|
stopMagHdgFusion();
|
|
zeroMagCov();
|
|
loadMagCovData();
|
|
_control_status.flags.mag_3D = true;
|
|
}
|
|
}
|
|
|
|
// update the estimated misalignment between the EV navigation frame and the EKF navigation frame
|
|
// and calculate a rotation matrix which rotates EV measurements into the EKF's navigation frame
|
|
void Ekf::calcExtVisRotMat()
|
|
{
|
|
// Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon.
|
|
Quatf q_error = _state.quat_nominal * _ev_sample_delayed.quat.inversed();
|
|
q_error.normalize();
|
|
|
|
// convert to a delta angle and apply a spike and low pass filter
|
|
Vector3f rot_vec = q_error.to_axis_angle();
|
|
|
|
float rot_vec_norm = rot_vec.norm();
|
|
|
|
if (rot_vec_norm > 1e-6f) {
|
|
|
|
// apply an input limiter to protect from spikes
|
|
Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt;
|
|
float input_delta_len = _input_delta_vec.norm();
|
|
|
|
if (input_delta_len > 0.1f) {
|
|
rot_vec = _ev_rot_vec_filt + _input_delta_vec * (0.1f / input_delta_len);
|
|
}
|
|
|
|
// Apply a first order IIR low pass filter
|
|
const float omega_lpf_us = 0.2e-6f; // cutoff frequency in rad/uSec
|
|
float alpha = math::constrain(omega_lpf_us * (float)(_time_last_imu - _ev_rot_last_time_us), 0.0f, 1.0f);
|
|
_ev_rot_last_time_us = _time_last_imu;
|
|
_ev_rot_vec_filt = _ev_rot_vec_filt * (1.0f - alpha) + rot_vec * alpha;
|
|
|
|
}
|
|
|
|
// convert filtered vector to a quaternion and then to a rotation matrix
|
|
q_error.from_axis_angle(_ev_rot_vec_filt);
|
|
_ev_rot_mat = Dcmf(q_error); // rotation from EV reference to EKF reference
|
|
|
|
}
|
|
|
|
// reset the estimated misalignment between the EV navigation frame and the EKF navigation frame
|
|
// and update the rotation matrix which rotates EV measurements into the EKF's navigation frame
|
|
void Ekf::resetExtVisRotMat()
|
|
{
|
|
// Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon.
|
|
Quatf q_error = _state.quat_nominal * _ev_sample_delayed.quat.inversed();
|
|
q_error.normalize();
|
|
|
|
// convert to a delta angle and reset
|
|
Vector3f rot_vec = q_error.to_axis_angle();
|
|
|
|
float rot_vec_norm = rot_vec.norm();
|
|
|
|
if (rot_vec_norm > 1e-9f) {
|
|
_ev_rot_vec_filt = rot_vec;
|
|
|
|
} else {
|
|
_ev_rot_vec_filt.zero();
|
|
}
|
|
|
|
// reset the rotation matrix
|
|
_ev_rot_mat = Dcmf(q_error); // rotation from EV reference to EKF reference
|
|
}
|
|
|
|
// return the quaternions for the rotation from External Vision system reference frame to the EKF reference frame
|
|
void Ekf::get_ev2ekf_quaternion(float *quat)
|
|
{
|
|
Quatf quat_ev2ekf;
|
|
quat_ev2ekf.from_axis_angle(_ev_rot_vec_filt);
|
|
|
|
for (unsigned i = 0; i < 4; i++) {
|
|
quat[i] = quat_ev2ekf(i);
|
|
}
|
|
}
|
|
|
|
// Increase the yaw error variance of the quaternions
|
|
// Argument is additional yaw variance in rad**2
|
|
void Ekf::increaseQuatYawErrVariance(float yaw_variance)
|
|
{
|
|
// See DeriveYawResetEquations.m for derivation which produces code fragments in C_code4.txt file
|
|
// The auto-code was cleaned up and had terms multiplied by zero removed to give the following:
|
|
|
|
// Intermediate variables
|
|
float SG[3];
|
|
SG[0] = sq(_state.quat_nominal(0)) - sq(_state.quat_nominal(1)) - sq(_state.quat_nominal(2)) + sq(_state.quat_nominal(3));
|
|
SG[1] = 2*_state.quat_nominal(0)*_state.quat_nominal(2) - 2*_state.quat_nominal(1)*_state.quat_nominal(3);
|
|
SG[2] = 2*_state.quat_nominal(0)*_state.quat_nominal(1) + 2*_state.quat_nominal(2)*_state.quat_nominal(3);
|
|
|
|
float SQ[4];
|
|
SQ[0] = 0.5f * ((_state.quat_nominal(1)*SG[0]) - (_state.quat_nominal(0)*SG[2]) + (_state.quat_nominal(3)*SG[1]));
|
|
SQ[1] = 0.5f * ((_state.quat_nominal(0)*SG[1]) - (_state.quat_nominal(2)*SG[0]) + (_state.quat_nominal(3)*SG[2]));
|
|
SQ[2] = 0.5f * ((_state.quat_nominal(3)*SG[0]) - (_state.quat_nominal(1)*SG[1]) + (_state.quat_nominal(2)*SG[2]));
|
|
SQ[3] = 0.5f * ((_state.quat_nominal(0)*SG[0]) + (_state.quat_nominal(1)*SG[2]) + (_state.quat_nominal(2)*SG[1]));
|
|
|
|
// Limit yaw variance increase to prevent a badly conditioned covariance matrix
|
|
yaw_variance = fminf(yaw_variance, 1.0e-2f);
|
|
|
|
// Add covariances for additonal yaw uncertainty to existing covariances.
|
|
// This assumes that the additional yaw error is uncorrrelated to existing errors
|
|
P[0][0] += yaw_variance*sq(SQ[2]);
|
|
P[0][1] += yaw_variance*SQ[1]*SQ[2];
|
|
P[1][1] += yaw_variance*sq(SQ[1]);
|
|
P[0][2] += yaw_variance*SQ[0]*SQ[2];
|
|
P[1][2] += yaw_variance*SQ[0]*SQ[1];
|
|
P[2][2] += yaw_variance*sq(SQ[0]);
|
|
P[0][3] -= yaw_variance*SQ[2]*SQ[3];
|
|
P[1][3] -= yaw_variance*SQ[1]*SQ[3];
|
|
P[2][3] -= yaw_variance*SQ[0]*SQ[3];
|
|
P[3][3] += yaw_variance*sq(SQ[3]);
|
|
P[1][0] += yaw_variance*SQ[1]*SQ[2];
|
|
P[2][0] += yaw_variance*SQ[0]*SQ[2];
|
|
P[2][1] += yaw_variance*SQ[0]*SQ[1];
|
|
P[3][0] -= yaw_variance*SQ[2]*SQ[3];
|
|
P[3][1] -= yaw_variance*SQ[1]*SQ[3];
|
|
P[3][2] -= yaw_variance*SQ[0]*SQ[3];
|
|
}
|
|
|
|
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
|
|
void Ekf::saveMagCovData()
|
|
{
|
|
// save variances for the D earth axis and XYZ body axis field
|
|
for (uint8_t index = 0; index <= 3; index ++) {
|
|
_saved_mag_bf_variance[index] = P[index + 18][index + 18];
|
|
}
|
|
|
|
// save the NE axis covariance sub-matrix
|
|
for (uint8_t row = 0; row <= 1; row ++) {
|
|
for (uint8_t col = 0; col <= 1; col ++) {
|
|
_saved_mag_ef_covmat[row][col] = P[row + 16][col + 16];
|
|
}
|
|
}
|
|
}
|
|
|
|
void Ekf::loadMagCovData()
|
|
{
|
|
// re-instate variances for the D earth axis and XYZ body axis field
|
|
for (uint8_t index = 0; index <= 3; index ++) {
|
|
P[index + 18][index + 18] = _saved_mag_bf_variance[index];
|
|
}
|
|
// re-instate the NE axis covariance sub-matrix
|
|
for (uint8_t row = 0; row <= 1; row ++) {
|
|
for (uint8_t col = 0; col <= 1; col ++) {
|
|
P[row + 16][col + 16] = _saved_mag_ef_covmat[row][col];
|
|
}
|
|
}
|
|
}
|
|
|
|
float Ekf::kahanSummation(float sum_previous, float input, float &accumulator) const
|
|
{
|
|
float y = input - accumulator;
|
|
float t = sum_previous + y;
|
|
accumulator = (t - sum_previous) - y;
|
|
return t;
|
|
}
|
|
|
|
|
|
void Ekf::stopGpsFusion()
|
|
{
|
|
stopGpsPosFusion();
|
|
stopGpsVelFusion();
|
|
stopGpsYawFusion();
|
|
}
|
|
|
|
void Ekf::stopGpsPosFusion()
|
|
{
|
|
_control_status.flags.gps = false;
|
|
_control_status.flags.gps_hgt = false;
|
|
memset(_gps_vel_pos_innov+3,0.0f, sizeof(float)*3);
|
|
memset(_gps_vel_pos_innov_var+3,0.0f, sizeof(float)*3);
|
|
memset(_gps_vel_pos_test_ratio+HPOS,0.0f, sizeof(float)*2);
|
|
}
|
|
|
|
void Ekf::stopGpsVelFusion()
|
|
{
|
|
memset(_gps_vel_pos_innov,0.0f, sizeof(float)*3);
|
|
memset(_gps_vel_pos_innov_var,0.0f, sizeof(float)*3);
|
|
memset(_gps_vel_pos_test_ratio,0.0f, sizeof(float)*2);
|
|
}
|
|
|
|
void Ekf::stopGpsYawFusion()
|
|
{
|
|
_control_status.flags.gps_yaw = false;
|
|
}
|
|
|
|
void Ekf::stopEvFusion()
|
|
{
|
|
stopEvPosFusion();
|
|
stopEvVelFusion();
|
|
stopEvYawFusion();
|
|
}
|
|
|
|
void Ekf::stopEvPosFusion()
|
|
{
|
|
_control_status.flags.ev_pos = false;
|
|
memset(_ev_vel_pos_innov+3,0.0f, sizeof(float)*3);
|
|
memset(_ev_vel_pos_innov_var+3,0.0f, sizeof(float)*3);
|
|
memset(_ev_vel_pos_test_ratio+HPOS,0.0f, sizeof(float)*2);
|
|
}
|
|
|
|
void Ekf::stopEvVelFusion()
|
|
{
|
|
_control_status.flags.ev_vel = false;
|
|
memset(_ev_vel_pos_innov,0.0f, sizeof(float)*3);
|
|
memset(_ev_vel_pos_innov_var,0.0f, sizeof(float)*3);
|
|
memset(_ev_vel_pos_test_ratio,0.0f, sizeof(float)*2);}
|
|
|
|
void Ekf::stopEvYawFusion()
|
|
{
|
|
_control_status.flags.ev_yaw = false;
|
|
|
|
}
|
|
|
|
void Ekf::stopAuxVelFusion()
|
|
{
|
|
// TODO: Add proper handling of auxiliar velocity fusion
|
|
}
|
|
|
|
void Ekf::stopFlowFusion()
|
|
{
|
|
_control_status.flags.opt_flow = false;
|
|
memset(_flow_innov,0.0f,sizeof(_flow_innov));
|
|
memset(_flow_innov_var,0.0f,sizeof(_flow_innov_var));
|
|
memset(&_optflow_test_ratio,0.0f,sizeof(_optflow_test_ratio));
|
|
}
|