forked from Archive/PX4-Autopilot
ekf2: pass gpsSample around where required
- this minimizes potential misuse accessing _gps_sample_delayed and makes the dependency clear
This commit is contained in:
Daniel Agar
parent
0c860fa227
commit
d996af4647
|
|
@ -566,7 +566,7 @@ void Ekf::resetOnGroundMotionForOpticalFlowChecks()
|
|||
_time_good_motion_us = _imu_sample_delayed.time_us;
|
||||
}
|
||||
|
||||
void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
|
||||
void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing)
|
||||
{
|
||||
if (!(_params.gnss_ctrl & GnssCtrl::YAW)
|
||||
|| _control_status.flags.gps_yaw_fault) {
|
||||
|
|
@ -575,7 +575,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
|
|||
return;
|
||||
}
|
||||
|
||||
const bool is_new_data_available = PX4_ISFINITE(_gps_sample_delayed.yaw);
|
||||
const bool is_new_data_available = PX4_ISFINITE(gps_sample.yaw);
|
||||
|
||||
if (is_new_data_available) {
|
||||
|
||||
|
|
@ -593,14 +593,14 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
|
|||
|
||||
if (continuing_conditions_passing) {
|
||||
|
||||
fuseGpsYaw();
|
||||
fuseGpsYaw(gps_sample);
|
||||
|
||||
const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);
|
||||
|
||||
if (is_fusion_failing) {
|
||||
if (_nb_gps_yaw_reset_available > 0) {
|
||||
// Data seems good, attempt a reset
|
||||
resetYawToGps();
|
||||
resetYawToGps(gps_sample.yaw);
|
||||
|
||||
if (_control_status.flags.in_air) {
|
||||
_nb_gps_yaw_reset_available--;
|
||||
|
|
@ -629,7 +629,7 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
|
|||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
// Try to activate GPS yaw fusion
|
||||
startGpsYawFusion();
|
||||
startGpsYawFusion(gps_sample);
|
||||
|
||||
if (_control_status.flags.gps_yaw) {
|
||||
_nb_gps_yaw_reset_available = 1;
|
||||
|
|
|
|||
|
|
@ -73,7 +73,7 @@ void Ekf::initialiseCovariance()
|
|||
P(9,9) = sq(fmaxf(_params.range_noise, 0.01f));
|
||||
|
||||
} else if (_control_status.flags.gps_hgt) {
|
||||
P(9,9) = getGpsHeightVariance();
|
||||
P(9,9) = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
|
||||
|
||||
} else {
|
||||
P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f));
|
||||
|
|
|
|||
|
|
@ -653,14 +653,14 @@ private:
|
|||
// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
|
||||
// innovation : prediction - measurement
|
||||
// variance : observaton variance
|
||||
bool fuseYaw(const float innovation, const float variance, estimator_aid_source_1d_s& aid_src_status);
|
||||
bool fuseYaw(const float innovation, const float variance, estimator_aid_source_1d_s &aid_src_status);
|
||||
|
||||
// fuse the yaw angle obtained from a dual antenna GPS unit
|
||||
void fuseGpsYaw();
|
||||
void fuseGpsYaw(const gpsSample &gps_sample);
|
||||
|
||||
// reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit
|
||||
// return true if the reset was successful
|
||||
bool resetYawToGps();
|
||||
bool resetYawToGps(const float gnss_yaw);
|
||||
|
||||
// fuse magnetometer declination measurement
|
||||
// argument passed in is the declination uncertainty in radians
|
||||
|
|
@ -692,12 +692,12 @@ private:
|
|||
void resetHorizontalVelocityTo(const Vector2f &new_horz_vel);
|
||||
void resetVerticalVelocityTo(float new_vert_vel);
|
||||
|
||||
void resetVelocityToGps(const gpsSample &gps_sample_delayed);
|
||||
void resetVelocityToGps(const gpsSample &gps_sample);
|
||||
void resetHorizontalVelocityToOpticalFlow();
|
||||
void resetVelocityToVision();
|
||||
void resetHorizontalVelocityToZero();
|
||||
|
||||
void resetHorizontalPositionToGps(const gpsSample &gps_sample_delayed);
|
||||
void resetHorizontalPositionToGps(const gpsSample &gps_sample);
|
||||
void resetHorizontalPositionToVision();
|
||||
void resetHorizontalPositionToOpticalFlow();
|
||||
void resetHorizontalPositionToLastKnown();
|
||||
|
|
@ -708,21 +708,23 @@ private:
|
|||
void resetVerticalPositionTo(float new_vert_pos);
|
||||
|
||||
void resetHeightToBaro();
|
||||
void resetHeightToGps();
|
||||
void resetHeightToGps(const gpsSample &gps_sample);
|
||||
void resetHeightToRng();
|
||||
void resetHeightToEv();
|
||||
|
||||
void resetVerticalVelocityToGps(const gpsSample &gps_sample_delayed);
|
||||
void resetVerticalVelocityToGps(const gpsSample &gps_sample);
|
||||
void resetVerticalVelocityToZero();
|
||||
|
||||
// fuse optical flow line of sight rate measurements
|
||||
void fuseOptFlow();
|
||||
|
||||
void updateVelocityAidSrcStatus(const uint64_t& sample_time_us, const Vector3f& velocity, const Vector3f& obs_var, const float innov_gate, estimator_aid_source_3d_s& vel_aid_src) const;
|
||||
void updatePositionAidSrcStatus(const uint64_t& sample_time_us, const Vector3f& position, const Vector3f& obs_var, const float innov_gate, estimator_aid_source_3d_s& pos_aid_src) const;
|
||||
void updateVelocityAidSrcStatus(const uint64_t &sample_time_us, const Vector3f &velocity, const Vector3f &obs_var,
|
||||
const float innov_gate, estimator_aid_source_3d_s &vel_aid_src) const;
|
||||
void updatePositionAidSrcStatus(const uint64_t &sample_time_us, const Vector3f &position, const Vector3f &obs_var,
|
||||
const float innov_gate, estimator_aid_source_3d_s &pos_aid_src) const;
|
||||
|
||||
void fuseVelocity(estimator_aid_source_3d_s& vel_aid_src);
|
||||
void fusePosition(estimator_aid_source_3d_s& pos_aid_src);
|
||||
void fuseVelocity(estimator_aid_source_3d_s &vel_aid_src);
|
||||
void fusePosition(estimator_aid_source_3d_s &pos_aid_src);
|
||||
|
||||
bool fuseHorizontalVelocity(const Vector3f &innov, float innov_gate, const Vector3f &obs_var,
|
||||
Vector3f &innov_var, Vector2f &test_ratio);
|
||||
|
|
@ -893,7 +895,7 @@ private:
|
|||
bool hasHorizontalAidingTimedOut() const;
|
||||
bool isYawFailure() const;
|
||||
|
||||
void controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing);
|
||||
void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing);
|
||||
|
||||
// control fusion of magnetometer observations
|
||||
void controlMagFusion();
|
||||
|
|
@ -945,7 +947,7 @@ private:
|
|||
void controlHeightFusion();
|
||||
void checkHeightSensorRefFallback();
|
||||
void controlBaroHeightFusion();
|
||||
void controlGnssHeightFusion();
|
||||
void controlGnssHeightFusion(const gpsSample &gps_sample);
|
||||
void controlRangeHeightFusion();
|
||||
void controlEvHeightFusion();
|
||||
|
||||
|
|
@ -960,7 +962,7 @@ private:
|
|||
void startBaroHgtFusion();
|
||||
void stopBaroHgtFusion();
|
||||
|
||||
void startGpsHgtFusion();
|
||||
void startGpsHgtFusion(const gpsSample &gps_sample);
|
||||
void stopGpsHgtFusion();
|
||||
|
||||
void startRngHgtFusion();
|
||||
|
|
@ -972,7 +974,7 @@ private:
|
|||
void updateGroundEffect();
|
||||
|
||||
// return an estimation of the sensor altitude variance
|
||||
float getGpsHeightVariance();
|
||||
float getGpsHeightVariance(const gpsSample &gps_sample);
|
||||
float getRngHeightVariance() const;
|
||||
|
||||
// calculate the measurement variance for the optical flow sensor
|
||||
|
|
@ -1040,12 +1042,12 @@ private:
|
|||
void startAirspeedFusion();
|
||||
void stopAirspeedFusion();
|
||||
|
||||
void startGpsFusion();
|
||||
void startGpsFusion(const gpsSample &gps_sample);
|
||||
void stopGpsFusion();
|
||||
void stopGpsPosFusion();
|
||||
void stopGpsVelFusion();
|
||||
|
||||
void startGpsYawFusion();
|
||||
void startGpsYawFusion(const gpsSample &gps_sample);
|
||||
void stopGpsYawFusion();
|
||||
|
||||
void startEvPosFusion();
|
||||
|
|
|
|||
|
|
@ -44,12 +44,12 @@
|
|||
#include <mathlib/mathlib.h>
|
||||
#include <cstdlib>
|
||||
|
||||
void Ekf::resetVelocityToGps(const gpsSample &gps_sample_delayed)
|
||||
void Ekf::resetVelocityToGps(const gpsSample &gps_sample)
|
||||
{
|
||||
_information_events.flags.reset_vel_to_gps = true;
|
||||
ECL_INFO("reset velocity to GPS");
|
||||
resetVelocityTo(gps_sample_delayed.vel);
|
||||
P.uncorrelateCovarianceSetVariance<3>(4, sq(gps_sample_delayed.sacc));
|
||||
resetVelocityTo(gps_sample.vel);
|
||||
P.uncorrelateCovarianceSetVariance<3>(4, sq(gps_sample.sacc));
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalVelocityToOpticalFlow()
|
||||
|
|
@ -144,12 +144,12 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel)
|
|||
_time_last_ver_vel_fuse = _imu_sample_delayed.time_us;
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionToGps(const gpsSample &gps_sample_delayed)
|
||||
void Ekf::resetHorizontalPositionToGps(const gpsSample &gps_sample)
|
||||
{
|
||||
_information_events.flags.reset_pos_to_gps = true;
|
||||
ECL_INFO("reset position to GPS");
|
||||
resetHorizontalPositionTo(gps_sample_delayed.pos);
|
||||
P.uncorrelateCovarianceSetVariance<2>(7, sq(gps_sample_delayed.hacc));
|
||||
resetHorizontalPositionTo(gps_sample.pos);
|
||||
P.uncorrelateCovarianceSetVariance<2>(7, sq(gps_sample.hacc));
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionToVision()
|
||||
|
|
@ -251,12 +251,12 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos)
|
|||
_time_last_hgt_fuse = _imu_sample_delayed.time_us;
|
||||
}
|
||||
|
||||
void Ekf::resetVerticalVelocityToGps(const gpsSample &gps_sample_delayed)
|
||||
void Ekf::resetVerticalVelocityToGps(const gpsSample &gps_sample)
|
||||
{
|
||||
resetVerticalVelocityTo(gps_sample_delayed.vel(2));
|
||||
resetVerticalVelocityTo(gps_sample.vel(2));
|
||||
|
||||
// the state variance is the same as the observation
|
||||
P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_sample_delayed.sacc));
|
||||
P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_sample.sacc));
|
||||
}
|
||||
|
||||
void Ekf::resetVerticalVelocityToZero()
|
||||
|
|
@ -1230,13 +1230,13 @@ void Ekf::startMag3DFusion()
|
|||
}
|
||||
}
|
||||
|
||||
float Ekf::getGpsHeightVariance()
|
||||
float Ekf::getGpsHeightVariance(const gpsSample &gps_sample)
|
||||
{
|
||||
// observation variance - receiver defined and parameter limited
|
||||
// use 1.5 as a typical ratio of vacc/hacc
|
||||
const float lower_limit = fmaxf(1.5f * _params.gps_pos_noise, 0.01f);
|
||||
const float upper_limit = fmaxf(1.5f * _params.pos_noaid_noise, lower_limit);
|
||||
const float gps_alt_var = sq(math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
|
||||
const float gps_alt_var = sq(math::constrain(gps_sample.vacc, lower_limit, upper_limit));
|
||||
return gps_alt_var;
|
||||
}
|
||||
|
||||
|
|
@ -1415,14 +1415,14 @@ void Ekf::stopAirspeedFusion()
|
|||
_control_status.flags.fuse_aspd = false;
|
||||
}
|
||||
|
||||
void Ekf::startGpsFusion()
|
||||
void Ekf::startGpsFusion(const gpsSample &gps_sample)
|
||||
{
|
||||
if (!_control_status.flags.gps) {
|
||||
resetHorizontalPositionToGps(_gps_sample_delayed);
|
||||
resetHorizontalPositionToGps(gps_sample);
|
||||
|
||||
// when already using another velocity source velocity reset is not necessary
|
||||
if (!_control_status.flags.opt_flow && !_control_status.flags.ev_vel) {
|
||||
resetVelocityToGps(_gps_sample_delayed);
|
||||
resetVelocityToGps(gps_sample);
|
||||
}
|
||||
|
||||
_information_events.flags.starting_gps_fusion = true;
|
||||
|
|
@ -1467,9 +1467,9 @@ void Ekf::stopGpsVelFusion()
|
|||
resetEstimatorAidStatus(_aid_src_gnss_vel);
|
||||
}
|
||||
|
||||
void Ekf::startGpsYawFusion()
|
||||
void Ekf::startGpsYawFusion(const gpsSample &gps_sample)
|
||||
{
|
||||
if (!_control_status.flags.gps_yaw && resetYawToGps()) {
|
||||
if (!_control_status.flags.gps_yaw && resetYawToGps(gps_sample.yaw)) {
|
||||
ECL_INFO("starting GPS yaw fusion");
|
||||
_control_status.flags.yaw_align = true;
|
||||
_control_status.flags.mag_dec = false;
|
||||
|
|
@ -1674,14 +1674,6 @@ void Ekf::runYawEKFGSF()
|
|||
|
||||
const Vector3f imu_gyro_bias = getGyroBias();
|
||||
_yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias);
|
||||
|
||||
// basic sanity check on GPS velocity data
|
||||
if (_gps_data_ready
|
||||
&& (_gps_sample_delayed.sacc > FLT_EPSILON) && (_gps_sample_delayed.sacc < _params.req_sacc)
|
||||
&& PX4_ISFINITE(_gps_sample_delayed.vel(0)) && PX4_ISFINITE(_gps_sample_delayed.vel(1))
|
||||
) {
|
||||
_yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.sacc);
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::resetGpsDriftCheckFilters()
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@
|
|||
|
||||
#include "ekf.h"
|
||||
|
||||
void Ekf::controlGnssHeightFusion()
|
||||
void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
|
||||
{
|
||||
if (!(_params.gnss_ctrl & GnssCtrl::VPOS)) {
|
||||
stopGpsHgtFusion();
|
||||
|
|
@ -59,8 +59,8 @@ void Ekf::controlGnssHeightFusion()
|
|||
|
||||
if (isHeightResetRequired()) {
|
||||
// All height sources are failing
|
||||
resetHeightToGps();
|
||||
resetVerticalVelocityToGps(_gps_sample_delayed);
|
||||
resetHeightToGps(gps_sample);
|
||||
resetVerticalVelocityToGps(gps_sample);
|
||||
|
||||
} else if (is_fusion_failing) {
|
||||
// Some other height source is still working
|
||||
|
|
@ -70,9 +70,10 @@ void Ekf::controlGnssHeightFusion()
|
|||
} else {
|
||||
stopGpsHgtFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
startGpsHgtFusion();
|
||||
startGpsHgtFusion(gps_sample);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -81,17 +82,17 @@ void Ekf::controlGnssHeightFusion()
|
|||
}
|
||||
}
|
||||
|
||||
void Ekf::startGpsHgtFusion()
|
||||
void Ekf::startGpsHgtFusion(const gpsSample &gps_sample)
|
||||
{
|
||||
if (!_control_status.flags.gps_hgt) {
|
||||
|
||||
if (_params.height_sensor_ref == HeightSensor::GNSS) {
|
||||
_gps_hgt_b_est.reset();
|
||||
_height_sensor_ref = HeightSensor::GNSS;
|
||||
resetHeightToGps();
|
||||
resetHeightToGps(gps_sample);
|
||||
|
||||
} else {
|
||||
_gps_hgt_b_est.setBias(_state.pos(2) + (_gps_sample_delayed.hgt - getEkfGlobalOriginAltitude()));
|
||||
_gps_hgt_b_est.setBias(_state.pos(2) + (gps_sample.hgt - getEkfGlobalOriginAltitude()));
|
||||
|
||||
// Reset the timeout value here because the fusion isn't done at the same place and would immediately trigger a timeout
|
||||
_aid_src_gnss_pos.time_last_fuse[2] = _imu_sample_delayed.time_us;
|
||||
|
|
@ -103,15 +104,15 @@ void Ekf::startGpsHgtFusion()
|
|||
}
|
||||
}
|
||||
|
||||
void Ekf::resetHeightToGps()
|
||||
void Ekf::resetHeightToGps(const gpsSample &gps_sample)
|
||||
{
|
||||
ECL_INFO("reset height to GPS");
|
||||
_information_events.flags.reset_hgt_to_gps = true;
|
||||
|
||||
resetVerticalPositionTo(-(_gps_sample_delayed.hgt - getEkfGlobalOriginAltitude() - _gps_hgt_b_est.getBias()));
|
||||
resetVerticalPositionTo(-(gps_sample.hgt - getEkfGlobalOriginAltitude() - _gps_hgt_b_est.getBias()));
|
||||
|
||||
// the state variance is the same as the observation
|
||||
P.uncorrelateCovarianceSetVariance<1>(9, getGpsHeightVariance());
|
||||
P.uncorrelateCovarianceSetVariance<1>(9, getGpsHeightVariance(gps_sample));
|
||||
|
||||
_baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change);
|
||||
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change);
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -49,14 +49,21 @@ void Ekf::controlGpsFusion()
|
|||
// Check for new GPS data that has fallen behind the fusion time horizon
|
||||
if (_gps_data_ready) {
|
||||
|
||||
updateGpsYaw(_gps_sample_delayed);
|
||||
updateGpsVel(_gps_sample_delayed);
|
||||
updateGpsPos(_gps_sample_delayed);
|
||||
const gpsSample &gps_sample{_gps_sample_delayed};
|
||||
|
||||
updateGpsYaw(gps_sample);
|
||||
updateGpsVel(gps_sample);
|
||||
updateGpsPos(gps_sample);
|
||||
|
||||
const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
|
||||
const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
|
||||
|
||||
controlGpsYawFusion(gps_checks_passing, gps_checks_failing);
|
||||
controlGpsYawFusion(gps_sample, gps_checks_passing, gps_checks_failing);
|
||||
|
||||
// update GSF yaw estimator velocity (basic sanity check on GNSS velocity data)
|
||||
if (gps_checks_passing && !gps_checks_failing) {
|
||||
_yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.sacc);
|
||||
}
|
||||
|
||||
// Determine if we should use GPS aiding for velocity and horizontal position
|
||||
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
|
||||
|
|
@ -144,7 +151,7 @@ void Ekf::controlGpsFusion()
|
|||
_inhibit_ev_yaw_use = true;
|
||||
|
||||
} else {
|
||||
startGpsFusion();
|
||||
startGpsFusion(gps_sample);
|
||||
}
|
||||
|
||||
} else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MagFuseType::NONE)) {
|
||||
|
|
@ -152,8 +159,8 @@ void Ekf::controlGpsFusion()
|
|||
if (resetYawToEKFGSF()) {
|
||||
_information_events.flags.yaw_aligned_to_imu_gps = true;
|
||||
ECL_INFO("Yaw aligned using IMU and GPS");
|
||||
resetVelocityToGps(_gps_sample_delayed);
|
||||
resetHorizontalPositionToGps(_gps_sample_delayed);
|
||||
resetVelocityToGps(gps_sample);
|
||||
resetHorizontalPositionToGps(gps_sample);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -48,7 +48,7 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample)
|
|||
// initially populate for estimator_aid_src_gnss_yaw logging
|
||||
|
||||
// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
|
||||
const float measured_hdg = wrap_pi(_gps_sample_delayed.yaw + _gps_yaw_offset);
|
||||
const float measured_hdg = wrap_pi(gps_sample.yaw + _gps_yaw_offset);
|
||||
|
||||
gps_yaw.observation = measured_hdg;
|
||||
gps_yaw.observation_variance = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f));
|
||||
|
|
@ -103,7 +103,7 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample)
|
|||
resetEstimatorAidStatus(gps_pos);
|
||||
|
||||
const float height_measurement = gps_sample.hgt - getEkfGlobalOriginAltitude();
|
||||
const float height_measurement_var = getGpsHeightVariance();
|
||||
const float height_measurement_var = getGpsHeightVariance(gps_sample);
|
||||
|
||||
Vector3f position;
|
||||
position(0) = gps_sample.pos(0);
|
||||
|
|
|
|||
|
|
@ -45,7 +45,7 @@
|
|||
#include <mathlib/mathlib.h>
|
||||
#include <cstdlib>
|
||||
|
||||
void Ekf::fuseGpsYaw()
|
||||
void Ekf::fuseGpsYaw(const gpsSample& gps_sample)
|
||||
{
|
||||
// assign intermediate state variables
|
||||
const float q0 = _state.quat_nominal(0);
|
||||
|
|
@ -54,7 +54,7 @@ void Ekf::fuseGpsYaw()
|
|||
const float q3 = _state.quat_nominal(3);
|
||||
|
||||
// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
|
||||
const float measured_hdg = wrap_pi(_gps_sample_delayed.yaw + _gps_yaw_offset);
|
||||
const float measured_hdg = wrap_pi(gps_sample.yaw + _gps_yaw_offset);
|
||||
|
||||
// define the predicted antenna array vector and rotate into earth frame
|
||||
const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
|
||||
|
|
@ -70,7 +70,7 @@ void Ekf::fuseGpsYaw()
|
|||
// TODO extend interface to use yaw uncertainty provided by GPS if available
|
||||
const float R_YAW = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f));
|
||||
|
||||
_aid_src_gnss_yaw.timestamp_sample = _gps_sample_delayed.time_us;
|
||||
_aid_src_gnss_yaw.timestamp_sample = gps_sample.time_us;
|
||||
_aid_src_gnss_yaw.observation = measured_hdg;
|
||||
_aid_src_gnss_yaw.observation_variance = R_YAW;
|
||||
_aid_src_gnss_yaw.innovation = heading_innov;
|
||||
|
|
@ -201,7 +201,7 @@ void Ekf::fuseGpsYaw()
|
|||
}
|
||||
}
|
||||
|
||||
bool Ekf::resetYawToGps()
|
||||
bool Ekf::resetYawToGps(const float gnss_yaw)
|
||||
{
|
||||
// define the predicted antenna array vector and rotate into earth frame
|
||||
const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
|
||||
|
|
@ -213,7 +213,7 @@ bool Ekf::resetYawToGps()
|
|||
}
|
||||
|
||||
// GPS yaw measurement is alreday compensated for antenna offset in the driver
|
||||
const float measured_yaw = _gps_sample_delayed.yaw;
|
||||
const float measured_yaw = gnss_yaw;
|
||||
|
||||
const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f));
|
||||
resetQuatStateYaw(measured_yaw, yaw_variance);
|
||||
|
|
|
|||
|
|
@ -44,7 +44,7 @@ void Ekf::controlHeightFusion()
|
|||
updateGroundEffect();
|
||||
|
||||
controlBaroHeightFusion();
|
||||
controlGnssHeightFusion();
|
||||
controlGnssHeightFusion(_gps_sample_delayed);
|
||||
controlRangeHeightFusion();
|
||||
controlEvHeightFusion();
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue