fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 04:51:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: fix computation of tilt and yaw variances add them to logging

Browse Source 
Co-authored-by: bresch <bresch@users.noreply.github.com>
This commit is contained in:
Mathieu Bresciani
2024-01-24 12:14:15 -05:00
committed by GitHub
co-authored by bresch
parent da28d9a7f2
commit cb396a6339
6 changed files with 272 additions and 258 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+3 -3
View File
@@ -255,7 +255,9 @@ public:
matrix::Vector<float, State::size> covariances_diagonal() const { return P.diag(); }
matrix::Vector<float, State::quat_nominal.dof> getQuaternionVariance() const { return getStateVariance<State::quat_nominal>(); }
float getTiltVariance() const { return getStateVariance<State::quat_nominal>()(0) + getStateVariance<State::quat_nominal>()(1); };
matrix::Vector3f getRotVarNed() const;
float getYawVar() const;
float getTiltVariance() const;
Vector3f getVelocityVariance() const { return getStateVariance<State::vel>(); };
@@ -410,8 +412,6 @@ public:
// return a bitmask integer that describes which state estimates can be used for flight control
void get_ekf_soln_status(uint16_t *status) const;
float getYawVar() const;
HeightSensor getHeightSensorRef() const { return _height_sensor_ref; }
#if defined(CONFIG_EKF2_AIRSPEED)
src/modules/ekf2/EKF/ekf_helper.cpp
+14 -5
View File
@@ -849,11 +849,21 @@ void Ekf::updateVerticalDeadReckoningStatus()
}
}
Vector3f Ekf::getRotVarNed() const
{
const matrix::SquareMatrix3f rot_cov_body = getStateCovariance<State::quat_nominal>();
return matrix::SquareMatrix<float, State::quat_nominal.dof>(_R_to_earth * rot_cov_body * _R_to_earth.T()).diag();
}
float Ekf::getYawVar() const
{
const matrix::SquareMatrix3f rot_cov = diag(getQuaternionVariance());
const auto rot_var_ned = matrix::SquareMatrix<float, State::quat_nominal.dof>(_R_to_earth * rot_cov * _R_to_earth.T()).diag();
return rot_var_ned(2);
return getRotVarNed()(2);
}
float Ekf::getTiltVariance() const
{
const Vector3f rot_var_ned = getRotVarNed();
return rot_var_ned(0) + rot_var_ned(1);
}
#if defined(CONFIG_EKF2_BAROMETER)
@@ -887,8 +897,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
const Quatf quat_before_reset = _state.quat_nominal;
// save a copy of covariance in NED frame to restore it after the quat reset
const matrix::SquareMatrix3f rot_cov = diag(getQuaternionVariance());
Vector3f rot_var_ned_before_reset = matrix::SquareMatrix3f(_R_to_earth * rot_cov * _R_to_earth.T()).diag();
Vector3f rot_var_ned_before_reset = getRotVarNed();
// update the yaw angle variance
if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) {