fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF2: ev-vel refactoring (#23577)

Browse Source
This commit is contained in:
Marco Hauswirth 2024-09-24 14:09:11 +02:00 committed by GitHub
parent 21a2350eca
commit 454ded1b25
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
12 changed files with 674 additions and 430 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d-posix/airframes/1013_gazebo-classic_iris_vision
View File

@ -5,7 +5,7 @@
# @type Quadrotor Wide
#
. ${R}etc/init.d-posix/airframes/10016_gazebo-classic_iris
. ${R}etc/init.d-posix/airframes/10015_gazebo-classic_iris
# EKF2: Vision position and heading, no GPS
param set-default EKF2_EV_DELAY 5

27
src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp
View File

@ -37,6 +37,7 @@
*/
#include "ekf.h"
#include "aid_sources/external_vision/ev_vel.h"
void Ekf::controlExternalVisionFusion(const imuSample &imu_sample)
{
@ -62,8 +63,32 @@ void Ekf::controlExternalVisionFusion(const imuSample &imu_sample)
&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
updateEvAttitudeErrorFilter(ev_sample, ev_reset);
controlEvYawFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw);
controlEvVelFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
switch (ev_sample.vel_frame) {
case VelocityFrame::BODY_FRAME_FRD: {
EvVelBodyFrameFrd ev_vel_body(*this, ev_sample, _params.ev_vel_noise, imu_sample);
controlEvVelFusion(ev_vel_body, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
break;
}
case VelocityFrame::LOCAL_FRAME_NED: {
EvVelLocalFrameNed ev_vel_ned(*this, ev_sample, _params.ev_vel_noise, imu_sample);
controlEvVelFusion(ev_vel_ned, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
break;
}
case VelocityFrame::LOCAL_FRAME_FRD: {
EvVelLocalFrameFrd ev_vel_frd(*this, ev_sample, _params.ev_vel_noise, imu_sample);
controlEvVelFusion(ev_vel_frd, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
break;
}
default:
return;
}
controlEvPosFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos);
controlEvHeightFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient,
_aid_src_ev_hgt);

176
src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel.h Normal file
View File

@ -0,0 +1,176 @@
/****************************************************************************
*
* Copyright (c) 2024 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
* 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 PX4 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 ev_vel.h
* Handling different types of External Vision velocity measurements
*/
#ifndef ExternalVisionVel_H
#define ExternalVisionVel_H
#include "ekf.h"
#include <uORB/topics/estimator_aid_source3d.h>
class ExternalVisionVel
{
public:
ExternalVisionVel(Ekf &ekf_instance, const extVisionSample &vision_sample, const float ev_vel_noise,
const imuSample &imu_sample) : _ekf(ekf_instance), _sample(vision_sample)
{
_min_variance = sq(ev_vel_noise);
const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _ekf._state.gyro_bias;
Vector3f position_offset_body = _ekf._params.ev_pos_body - _ekf._params.imu_pos_body;
_velocity_offset_body = angular_velocity % position_offset_body;
}
virtual ~ExternalVisionVel() = default;
virtual bool fuseVelocity(estimator_aid_source3d_s &aid_src, float gate)
{
_ekf.fuseLocalFrameVelocity(aid_src, aid_src.timestamp, _measurement,
_measurement_var, gate);
return aid_src.fused;
}
virtual void resetVelocity()
{
_ekf.resetVelocityTo(_measurement, _measurement_var);
}
void resetVerticalVelocity()
{
_ekf.resetVerticalVelocityTo(_measurement(2), _measurement_var(2));
}
void enforceMinimumVariance()
{
_min_variance = math::max(_min_variance, sq(0.01f));
for (int i = 0; i < 3; ++i) {
_measurement_var(i) = math::max(_measurement_var(i), _min_variance);
}
}
Ekf &_ekf;
const extVisionSample &_sample;
float _min_variance;
Vector3f _measurement{NAN, NAN, NAN};
Vector3f _measurement_var;
Vector3f _velocity_offset_body;
};
class EvVelBodyFrameFrd : public ExternalVisionVel
{
public:
EvVelBodyFrameFrd(Ekf &ekf_instance, extVisionSample &vision_sample, const float ev_vel_noise,
const imuSample &imu_sample) :
ExternalVisionVel(ekf_instance, vision_sample, ev_vel_noise, imu_sample)
{
_measurement = _sample.vel - _velocity_offset_body;
_measurement_var = _sample.velocity_var;
enforceMinimumVariance();
}
void resetVelocity() override
{
const matrix::SquareMatrix3f rotated_variance = _ekf._R_to_earth * matrix::diag(
_measurement_var) * _ekf._R_to_earth.transpose();
// bump the variance to decrease cross-correlation and increase uncertainty of velocity
const Vector3f measurement_variance_ekf_frame = rotated_variance.diag() * 5.f;
_ekf.resetVelocityTo(_ekf._R_to_earth * _measurement, measurement_variance_ekf_frame);
}
void resetVerticalVelocity()
{
const matrix::SquareMatrix3f rotated_variance = _ekf._R_to_earth * matrix::diag(
_measurement_var) * _ekf._R_to_earth.transpose();
// bump the variance to decrease cross-correlation and increase uncertainty of velocity
const Vector3f measurement_variance_ekf_frame = rotated_variance.diag() * 5.f;
_ekf.resetVerticalVelocityTo((_ekf._R_to_earth * _measurement)(2, 0),
measurement_variance_ekf_frame(2));
}
bool fuseVelocity(estimator_aid_source3d_s &aid_src, float gate) override
{
_ekf.fuseBodyFrameVelocity(aid_src, _sample.time_us, _measurement,
_measurement_var, gate);
return aid_src.fused;
}
};
class EvVelLocalFrameNed : public ExternalVisionVel
{
public:
EvVelLocalFrameNed(Ekf &ekf_instance, extVisionSample &vision_sample, const float ev_vel_noise,
const imuSample &imu_sample) :
ExternalVisionVel(ekf_instance, vision_sample, ev_vel_noise, imu_sample)
{
const Vector3f velocity_offset_earth = _ekf._R_to_earth * _velocity_offset_body;
if (_ekf._control_status.flags.yaw_align) {
_measurement = _sample.vel - velocity_offset_earth;
_measurement_var = _sample.velocity_var;
enforceMinimumVariance();
}
}
};
class EvVelLocalFrameFrd : public ExternalVisionVel
{
public:
EvVelLocalFrameFrd(Ekf &ekf_instance, extVisionSample &vision_sample, const float ev_vel_noise,
const imuSample &imu_sample) :
ExternalVisionVel(ekf_instance, vision_sample, ev_vel_noise, imu_sample)
{
const Vector3f velocity_offset_earth = _ekf._R_to_earth * _velocity_offset_body;
if (_ekf._control_status.flags.ev_yaw) {
// Using EV frame
_measurement = _sample.vel - velocity_offset_earth;
_measurement_var = _sample.velocity_var;
} else {
// Rotate EV to the EKF reference frame
const Dcmf rotation_ev_to_ekf = Dcmf(_ekf._ev_q_error_filt.getState());
_measurement = rotation_ev_to_ekf * _sample.vel - velocity_offset_earth;
_measurement_var = matrix::SquareMatrix3f(rotation_ev_to_ekf * matrix::diag(
_sample.velocity_var) * rotation_ev_to_ekf.transpose()).diag();
_min_variance = math::max(_min_variance, _sample.orientation_var.max());
}
enforceMinimumVariance();
}
};
#endif // ExternalVisionVel_H

283
src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp
View File

@ -37,13 +37,13 @@
*/
#include "ekf.h"
#include <ekf_derivation/generated/compute_ev_body_vel_hx.h>
#include <ekf_derivation/generated/compute_ev_body_vel_hy.h>
#include <ekf_derivation/generated/compute_ev_body_vel_hz.h>
#include "aid_sources/external_vision/ev_vel.h"
#include "ekf_derivation/generated/compute_body_vel_innov_var_h.h"
#include "ekf_derivation/generated/compute_body_vel_y_innov_var.h"
#include "ekf_derivation/generated/compute_body_vel_z_innov_var.h"
void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
estimator_aid_source3d_s &aid_src)
void Ekf::controlEvVelFusion(ExternalVisionVel &ev, const bool common_starting_conditions_passing, const bool ev_reset,
const bool quality_sufficient, estimator_aid_source3d_s &aid_src)
{
static constexpr const char *AID_SRC_NAME = "EV velocity";
@ -53,110 +53,12 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample
// determine if we should use EV velocity aiding
bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))
&& _control_status.flags.tilt_align
&& ev_sample.vel.isAllFinite();
// correct velocity for offset relative to IMU
const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias;
const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
const Vector3f vel_offset_body = angular_velocity % pos_offset_body;
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
// rotate measurement into correct earth frame if required
Vector3f measurement{};
Vector3f measurement_var{};
float minimum_variance = math::max(sq(0.01f), sq(_params.ev_vel_noise));
switch (ev_sample.vel_frame) {
case VelocityFrame::LOCAL_FRAME_NED:
if (_control_status.flags.yaw_align) {
measurement = ev_sample.vel - vel_offset_earth;
measurement_var = ev_sample.velocity_var;
} else {
continuing_conditions_passing = false;
}
break;
case VelocityFrame::LOCAL_FRAME_FRD:
if (_control_status.flags.ev_yaw) {
// using EV frame
measurement = ev_sample.vel - vel_offset_earth;
measurement_var = ev_sample.velocity_var;
} else {
// rotate EV to the EKF reference frame
const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState());
measurement = R_ev_to_ekf * ev_sample.vel - vel_offset_earth;
measurement_var = matrix::SquareMatrix3f(R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) *
R_ev_to_ekf.transpose()).diag();
minimum_variance = math::max(minimum_variance, ev_sample.orientation_var.max());
}
break;
case VelocityFrame::BODY_FRAME_FRD: {
// currently it is assumed that the orientation of the EV frame and the body frame are the same
measurement = ev_sample.vel - vel_offset_body;
measurement_var = ev_sample.velocity_var;
break;
}
default:
continuing_conditions_passing = false;
break;
}
#if defined(CONFIG_EKF2_GNSS)
// increase minimum variance if GPS active (position reference)
if (_control_status.flags.gps) {
for (int i = 0; i < 2; i++) {
measurement_var(i) = math::max(measurement_var(i), sq(_params.gps_vel_noise));
}
}
#endif // CONFIG_EKF2_GNSS
measurement_var = Vector3f{
math::max(measurement_var(0), minimum_variance),
math::max(measurement_var(1), minimum_variance),
math::max(measurement_var(2), minimum_variance)
};
continuing_conditions_passing &= measurement.isAllFinite() && measurement_var.isAllFinite();
if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) {
const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var);
const Vector3f measurement_ekf_frame = _R_to_earth * measurement;
const uint64_t t = aid_src.timestamp_sample;
updateAidSourceStatus(aid_src,
ev_sample.time_us, // sample timestamp
measurement_ekf_frame, // observation
measurement_var_ekf_frame, // observation variance
_state.vel - measurement_ekf_frame, // innovation
getVelocityVariance() + measurement_var_ekf_frame, // innovation variance
math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate
aid_src.timestamp_sample = t;
measurement.copyTo(aid_src.observation);
measurement_var.copyTo(aid_src.observation_variance);
} else {
updateAidSourceStatus(aid_src,
ev_sample.time_us, // sample timestamp
measurement, // observation
measurement_var, // observation variance
_state.vel - measurement, // innovation
getVelocityVariance() + measurement_var, // innovation variance
math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate
}
&& ev._sample.vel.isAllFinite();
const bool starting_conditions_passing = common_starting_conditions_passing
&& continuing_conditions_passing
&& ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive());
continuing_conditions_passing &= ev._measurement.isAllFinite() && ev._measurement_var.isAllFinite();
float gate = math::max(_params.ev_vel_innov_gate, 1.f);
if (_control_status.flags.ev_vel) {
if (continuing_conditions_passing) {
@ -164,7 +66,7 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample
if (quality_sufficient) {
ECL_INFO("reset to %s", AID_SRC_NAME);
_information_events.flags.reset_vel_to_vision = true;
resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame);
ev.resetVelocity();
resetAidSourceStatusZeroInnovation(aid_src);
} else {
@ -175,7 +77,7 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample
}
} else if (quality_sufficient) {
fuseEvVelocity(aid_src, ev_sample);
ev.fuseVelocity(aid_src, gate);
} else {
aid_src.innovation_rejected = true;
@ -186,10 +88,9 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample
if (is_fusion_failing) {
if ((_nb_ev_vel_reset_available > 0) && quality_sufficient) {
// Data seems good, attempt a reset
_information_events.flags.reset_vel_to_vision = true;
ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame);
ev.resetVelocity();
resetAidSourceStatusZeroInnovation(aid_src);
if (_control_status.flags.in_air) {
@ -197,32 +98,14 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample
}
} else {
// differ warning message based on whether the starting conditions are passing
if (starting_conditions_passing) {
// Data seems good, but previous reset did not fix the issue
// something else must be wrong, declare the sensor faulty and stop the fusion
//_control_status.flags.ev_vel_fault = true;
ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
} else {
// A reset did not fix the issue but all the starting checks are not passing
// This could be a temporary issue, stop the fusion without declaring the sensor faulty
ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
}
stopEvVelFusion();
}
} else if (isHeightResetRequired()) {
// reset vertical velocity if height is failing
if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) {
const Vector3f measurement_ekf_frame = _R_to_earth * measurement;
const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var);
resetVerticalVelocityTo(measurement_ekf_frame(2), measurement_var_ekf_frame(2));
} else {
resetVerticalVelocityTo(measurement(2), measurement_var(2));
}
ev.resetVerticalVelocity();
}
} else {
@ -232,18 +115,23 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample
}
} else {
if (starting_conditions_passing) {
if (common_starting_conditions_passing && continuing_conditions_passing) {
// make starting condition more sensitive if horizontal aiding is active
gate = isHorizontalAidingActive() ? (float)sqrt(10.f) * gate : gate;
// activate fusion, only reset if necessary
if (!isHorizontalAidingActive() || yaw_alignment_changed) {
ECL_INFO("starting %s fusion, resetting velocity to (%.3f, %.3f, %.3f)", AID_SRC_NAME,
(double)measurement(0), (double)measurement(1), (double)measurement(2));
(double)ev._measurement(0), (double)ev._measurement(1),
(double)ev._measurement(2));
_information_events.flags.reset_vel_to_vision = true;
resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame);
ev.resetVelocity();
resetAidSourceStatusZeroInnovation(aid_src);
_control_status.flags.ev_vel = true;
} else if (fuseEvVelocity(aid_src, ev_sample)) {
} else if (ev.fuseVelocity(aid_src, gate)) {
ECL_INFO("starting %s fusion", AID_SRC_NAME);
_control_status.flags.ev_vel = true;
}
@ -256,92 +144,65 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample
}
}
bool Ekf::fuseEvVelocity(estimator_aid_source3d_s &aid_src, const extVisionSample &ev_sample)
void Ekf::stopEvVelFusion()
{
if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) {
if (_control_status.flags.ev_vel) {
_control_status.flags.ev_vel = false;
}
}
VectorState H;
estimator_aid_source1d_s current_aid_src;
const auto state_vector = _state.vector();
void Ekf::fuseLocalFrameVelocity(estimator_aid_source3d_s &aid_src, const uint64_t &timestamp,
const Vector3f &measurement, const Vector3f &measurement_var, const float &innovation_gate)
{
updateAidSourceStatus(aid_src,
timestamp, // sample timestamp
measurement, // observation
measurement_var, // observation variance
_state.vel - measurement, // innovation
getVelocityVariance() + measurement_var, // innovation variance
innovation_gate); // innovation gate
fuseVelocity(aid_src);
}
void Ekf::fuseBodyFrameVelocity(estimator_aid_source3d_s &aid_src, const uint64_t &timestamp,
const Vector3f &measurement, const Vector3f &measurement_var, const float &innovation_gate)
{
VectorState H[3];
Vector3f innov_var;
Vector3f innov = _R_to_earth.transpose() * _state.vel - measurement;
const auto state_vector = _state.vector();
sym::ComputeBodyVelInnovVarH(state_vector, P, measurement_var, &innov_var, &H[0], &H[1], &H[2]);
updateAidSourceStatus(aid_src,
timestamp, // sample timestamp
measurement, // observation
measurement_var, // observation variance
innov, // innovation
innov_var, // innovation variance
innovation_gate); // innovation gate
if (!aid_src.innovation_rejected) {
aid_src.fused = true;
for (uint8_t index = 0; index <= 2; index++) {
current_aid_src.timestamp_sample = aid_src.timestamp_sample;
if (index == 0) {
sym::ComputeEvBodyVelHx(state_vector, &H);
} else if (index == 1) {
sym::ComputeEvBodyVelHy(state_vector, &H);
} else {
sym::ComputeEvBodyVelHz(state_vector, &H);
}
const float innov_var = (H.T() * P * H)(0, 0) + aid_src.observation_variance[index];
const float innov = (_R_to_earth.transpose() * _state.vel - Vector3f(aid_src.observation))(index, 0);
updateAidSourceStatus(current_aid_src,
ev_sample.time_us, // sample timestamp
aid_src.observation[index], // observation
aid_src.observation_variance[index], // observation variance
innov, // innovation
innov_var, // innovation variance
math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate
if (!current_aid_src.innovation_rejected) {
fuseBodyVelocity(current_aid_src, current_aid_src.innovation_variance, H);
}
aid_src.innovation[index] = current_aid_src.innovation;
aid_src.innovation_variance[index] = current_aid_src.innovation_variance;
aid_src.test_ratio[index] = current_aid_src.test_ratio;
aid_src.fused = current_aid_src.fused;
aid_src.innovation_rejected |= current_aid_src.innovation_rejected;
if (aid_src.fused) {
aid_src.time_last_fuse = _time_delayed_us;
}
if (index == 1) {
sym::ComputeBodyVelYInnovVar(state_vector, P, measurement_var(index), &aid_src.innovation_variance[index]);
} else if (index == 2) {
sym::ComputeBodyVelZInnovVar(state_vector, P, measurement_var(index), &aid_src.innovation_variance[index]);
}
aid_src.innovation[index] = Vector3f(_R_to_earth.transpose().row(index)) * _state.vel - measurement(index);
VectorState Kfusion = P * H[index] / aid_src.innovation_variance[index];
aid_src.fused &= measurementUpdate(Kfusion, H[index], aid_src.observation_variance[index], aid_src.innovation[index]);
}
}
if (aid_src.fused) {
_time_last_hor_vel_fuse = _time_delayed_us;
_time_last_ver_vel_fuse = _time_delayed_us;
aid_src.time_last_fuse = _time_delayed_us;
}
aid_src.timestamp_sample = current_aid_src.timestamp_sample;
return !aid_src.innovation_rejected;
} else {
return fuseVelocity(aid_src);
}
}
void Ekf::stopEvVelFusion()
{
if (_control_status.flags.ev_vel) {
_control_status.flags.ev_vel = false;
}
}
void Ekf::resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var,
const VelocityFrame &vel_frame)
{
if (vel_frame == VelocityFrame::BODY_FRAME_FRD) {
const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var);
resetVelocityTo(_R_to_earth * measurement, measurement_var_ekf_frame);
} else {
resetVelocityTo(measurement, measurement_var);
}
}
Vector3f Ekf::rotateVarianceToEkf(const Vector3f &measurement_var)
{
// rotate the covariance matrix into the EKF frame
const matrix::SquareMatrix<float, 3> R_cov = _R_to_earth * matrix::diag(measurement_var) * _R_to_earth.transpose();
return R_cov.diag();
}

17
src/modules/ekf2/EKF/ekf.h
View File

@ -67,6 +67,7 @@
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
enum class Likelihood { LOW, MEDIUM, HIGH };
class ExternalVisionVel;
class Ekf final : public EstimatorInterface
{
@ -422,6 +423,11 @@ public:
private:
friend class ExternalVisionVel;
friend class EvVelBodyFrameFrd;
friend class EvVelLocalFrameNed;
friend class EvVelLocalFrameFrd;
// set the internal states and status to their default value
void reset();
@ -807,14 +813,15 @@ private:
void controlEvPosFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
estimator_aid_source2d_s &aid_src);
void controlEvVelFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
estimator_aid_source3d_s &aid_src);
void controlEvVelFusion(ExternalVisionVel &ev, const bool common_starting_conditions_passing, const bool ev_reset,
const bool quality_sufficient, estimator_aid_source3d_s &aid_src);
void controlEvYawFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
estimator_aid_source1d_s &aid_src);
void resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, const VelocityFrame &vel_frame);
Vector3f rotateVarianceToEkf(const Vector3f &measurement_var);
void fuseLocalFrameVelocity(estimator_aid_source3d_s &aid_src, const uint64_t &timestamp, const Vector3f &measurement,
const Vector3f &measurement_var, const float &innovation_gate);
void fuseBodyFrameVelocity(estimator_aid_source3d_s &aid_src, const uint64_t &timestamp, const Vector3f &measurement,
const Vector3f &measurement_var, const float &innovation_gate);
void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src);
void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient,

51
src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
View File

@ -383,32 +383,45 @@ def predict_vel_body(
R_to_body = state["quat_nominal"].inverse()
return R_to_body * vel
def compute_ev_body_vel_hx(
def compute_body_vel_innov_var_h(
state: VState,
) -> (VTangent):
P: MTangent,
R: sf.V3,
) -> (sf.V3, VTangent, VTangent, VTangent):
state = vstate_to_state(state)
meas_pred = predict_vel_body(state)
Hx = jacobian_chain_rule(meas_pred[0], state)
return (Hx.T)
Hy = jacobian_chain_rule(meas_pred[1], state)
Hz = jacobian_chain_rule(meas_pred[2], state)
innov_var = sf.V3((Hx * P * Hx.T + R[0])[0,0],
(Hy * P * Hy.T + R[1])[0,0],
(Hz * P * Hz.T + R[2])[0,0])
def compute_ev_body_vel_hy(
return (innov_var, Hx.T, Hy.T, Hz.T)
def compute_body_vel_y_innov_var(
state: VState,
) -> (VTangent):
P: MTangent,
R: sf.Scalar
) -> (sf.Scalar):
state = vstate_to_state(state)
meas_pred = predict_vel_body(state)[1]
Hy = jacobian_chain_rule(meas_pred, state)
return (Hy.T)
meas_pred = predict_vel_body(state)
Hy = jacobian_chain_rule(meas_pred[1], state)
innov_var = (Hy * P * Hy.T + R)[0,0]
def compute_ev_body_vel_hz(
return (innov_var)
def compute_body_vel_z_innov_var(
state: VState,
) -> (VTangent):
P: MTangent,
R: sf.Scalar
) -> (sf.Scalar):
state = vstate_to_state(state)
meas_pred = predict_vel_body(state)[2]
Hz = jacobian_chain_rule(meas_pred, state)
return (Hz.T)
meas_pred = predict_vel_body(state)
Hz = jacobian_chain_rule(meas_pred[2], state)
innov_var = (Hz * P * Hz.T + R)[0,0]
return (innov_var)
def predict_mag_body(state) -> sf.V3:
mag_field_earth = state["mag_I"]
@ -746,8 +759,8 @@ generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas
generate_px4_function(compute_gravity_xyz_innov_var_and_hx, output_names=["innov_var", "Hx"])
generate_px4_function(compute_gravity_y_innov_var_and_h, output_names=["innov_var", "Hy"])
generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_var", "Hz"])
generate_px4_function(compute_ev_body_vel_hx, output_names=["H"])
generate_px4_function(compute_ev_body_vel_hy, output_names=["H"])
generate_px4_function(compute_ev_body_vel_hz, output_names=["H"])
generate_px4_function(compute_body_vel_innov_var_h, output_names=["innov_var", "Hx", "Hy", "Hz"])
generate_px4_function(compute_body_vel_y_innov_var, output_names=["innov_var"])
generate_px4_function(compute_body_vel_z_innov_var, output_names=["innov_var"])
generate_px4_state(State, tangent_idx)

192
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_body_vel_innov_var_h.h Normal file
View File

@ -0,0 +1,192 @@
// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
// function/FUNCTION.h.jinja
// Do NOT modify by hand.
// -----------------------------------------------------------------------------
#pragma once
#include <matrix/math.hpp>
namespace sym {
/**
* This function was autogenerated from a symbolic function. Do not modify by hand.
*
* Symbolic function: compute_body_vel_innov_var_h
*
* Args:
* state: Matrix25_1
* P: Matrix24_24
* R: Matrix31
*
* Outputs:
* innov_var: Matrix31
* Hx: Matrix24_1
* Hy: Matrix24_1
* Hz: Matrix24_1
*/
template <typename Scalar>
void ComputeBodyVelInnovVarH(const matrix::Matrix<Scalar, 25, 1>& state,
const matrix::Matrix<Scalar, 24, 24>& P,
const matrix::Matrix<Scalar, 3, 1>& R,
matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
matrix::Matrix<Scalar, 24, 1>* const Hx = nullptr,
matrix::Matrix<Scalar, 24, 1>* const Hy = nullptr,
matrix::Matrix<Scalar, 24, 1>* const Hz = nullptr) {
// Total ops: 371
// Input arrays
// Intermediate terms (63)
const Scalar _tmp0 = 2 * state(5, 0);
const Scalar _tmp1 = _tmp0 * state(3, 0);
const Scalar _tmp2 = 2 * state(6, 0);
const Scalar _tmp3 = _tmp2 * state(2, 0);
const Scalar _tmp4 = _tmp1 - _tmp3;
const Scalar _tmp5 = (Scalar(1) / Scalar(2)) * state(2, 0);
const Scalar _tmp6 = _tmp0 * state(2, 0);
const Scalar _tmp7 = _tmp2 * state(3, 0);
const Scalar _tmp8 = _tmp6 + _tmp7;
const Scalar _tmp9 = (Scalar(1) / Scalar(2)) * state(3, 0);
const Scalar _tmp10 = 4 * state(4, 0);
const Scalar _tmp11 = _tmp0 * state(0, 0);
const Scalar _tmp12 = 2 * state(1, 0);
const Scalar _tmp13 = _tmp12 * state(6, 0);
const Scalar _tmp14 = -_tmp10 * state(3, 0) + _tmp11 + _tmp13;
const Scalar _tmp15 = (Scalar(1) / Scalar(2)) * state(1, 0);
const Scalar _tmp16 = _tmp0 * state(1, 0);
const Scalar _tmp17 = _tmp2 * state(0, 0);
const Scalar _tmp18 = -_tmp10 * state(2, 0) + _tmp16 - _tmp17;
const Scalar _tmp19 = (Scalar(1) / Scalar(2)) * state(0, 0);
const Scalar _tmp20 = -_tmp14 * _tmp15 + _tmp18 * _tmp19 - _tmp4 * _tmp5 + _tmp8 * _tmp9;
const Scalar _tmp21 = 2 * state(2, 0);
const Scalar _tmp22 = _tmp21 * state(0, 0);
const Scalar _tmp23 = _tmp12 * state(3, 0);
const Scalar _tmp24 = -_tmp22 + _tmp23;
const Scalar _tmp25 = 2 * state(0, 0) * state(3, 0);
const Scalar _tmp26 = _tmp12 * state(2, 0);
const Scalar _tmp27 = _tmp25 + _tmp26;
const Scalar _tmp28 = -2 * std::pow(state(3, 0), Scalar(2));
const Scalar _tmp29 = 1 - 2 * std::pow(state(2, 0), Scalar(2));
const Scalar _tmp30 = _tmp28 + _tmp29;
const Scalar _tmp31 = _tmp14 * _tmp19 + _tmp15 * _tmp18 - _tmp4 * _tmp9 - _tmp5 * _tmp8;
const Scalar _tmp32 = _tmp14 * _tmp5 - _tmp15 * _tmp4 - _tmp18 * _tmp9 + _tmp19 * _tmp8;
const Scalar _tmp33 = -_tmp25 + _tmp26;
const Scalar _tmp34 = _tmp21 * state(3, 0);
const Scalar _tmp35 = _tmp12 * state(0, 0);
const Scalar _tmp36 = _tmp34 + _tmp35;
const Scalar _tmp37 = -2 * std::pow(state(1, 0), Scalar(2));
const Scalar _tmp38 = _tmp28 + _tmp37 + 1;
const Scalar _tmp39 = 2 * state(4, 0);
const Scalar _tmp40 = _tmp39 * state(3, 0);
const Scalar _tmp41 = _tmp13 - _tmp40;
const Scalar _tmp42 = _tmp39 * state(1, 0);
const Scalar _tmp43 = _tmp42 + _tmp7;
const Scalar _tmp44 = _tmp39 * state(0, 0);
const Scalar _tmp45 = 4 * state(5, 0);
const Scalar _tmp46 = _tmp3 - _tmp44 - _tmp45 * state(3, 0);
const Scalar _tmp47 = _tmp39 * state(2, 0);
const Scalar _tmp48 = _tmp17 - _tmp45 * state(1, 0) + _tmp47;
const Scalar _tmp49 = _tmp15 * _tmp43 + _tmp19 * _tmp46 - _tmp41 * _tmp9 - _tmp48 * _tmp5;
const Scalar _tmp50 = -_tmp15 * _tmp46 + _tmp19 * _tmp43 - _tmp41 * _tmp5 + _tmp48 * _tmp9;
const Scalar _tmp51 = -_tmp15 * _tmp41 + _tmp19 * _tmp48 - _tmp43 * _tmp9 + _tmp46 * _tmp5;
const Scalar _tmp52 = _tmp42 + _tmp6;
const Scalar _tmp53 = -_tmp16 + _tmp47;
const Scalar _tmp54 = 4 * state(6, 0);
const Scalar _tmp55 = _tmp1 + _tmp44 - _tmp54 * state(2, 0);
const Scalar _tmp56 = -_tmp11 + _tmp40 - _tmp54 * state(1, 0);
const Scalar _tmp57 = -_tmp15 * _tmp52 + _tmp19 * _tmp55 - _tmp5 * _tmp53 + _tmp56 * _tmp9;
const Scalar _tmp58 = _tmp34 - _tmp35;
const Scalar _tmp59 = _tmp22 + _tmp23;
const Scalar _tmp60 = _tmp29 + _tmp37;
const Scalar _tmp61 = _tmp15 * _tmp55 + _tmp19 * _tmp52 - _tmp5 * _tmp56 - _tmp53 * _tmp9;
const Scalar _tmp62 = -_tmp15 * _tmp53 + _tmp19 * _tmp56 + _tmp5 * _tmp52 - _tmp55 * _tmp9;
// Output terms (4)
if (innov_var != nullptr) {
matrix::Matrix<Scalar, 3, 1>& _innov_var = (*innov_var);
_innov_var(0, 0) = R(0, 0) +
_tmp20 * (P(0, 1) * _tmp32 + P(1, 1) * _tmp20 + P(2, 1) * _tmp31 +
P(3, 1) * _tmp30 + P(4, 1) * _tmp27 + P(5, 1) * _tmp24) +
_tmp24 * (P(0, 5) * _tmp32 + P(1, 5) * _tmp20 + P(2, 5) * _tmp31 +
P(3, 5) * _tmp30 + P(4, 5) * _tmp27 + P(5, 5) * _tmp24) +
_tmp27 * (P(0, 4) * _tmp32 + P(1, 4) * _tmp20 + P(2, 4) * _tmp31 +
P(3, 4) * _tmp30 + P(4, 4) * _tmp27 + P(5, 4) * _tmp24) +
_tmp30 * (P(0, 3) * _tmp32 + P(1, 3) * _tmp20 + P(2, 3) * _tmp31 +
P(3, 3) * _tmp30 + P(4, 3) * _tmp27 + P(5, 3) * _tmp24) +
_tmp31 * (P(0, 2) * _tmp32 + P(1, 2) * _tmp20 + P(2, 2) * _tmp31 +
P(3, 2) * _tmp30 + P(4, 2) * _tmp27 + P(5, 2) * _tmp24) +
_tmp32 * (P(0, 0) * _tmp32 + P(1, 0) * _tmp20 + P(2, 0) * _tmp31 +
P(3, 0) * _tmp30 + P(4, 0) * _tmp27 + P(5, 0) * _tmp24);
_innov_var(1, 0) = R(1, 0) +
_tmp33 * (P(0, 3) * _tmp51 + P(1, 3) * _tmp50 + P(2, 3) * _tmp49 +
P(3, 3) * _tmp33 + P(4, 3) * _tmp38 + P(5, 3) * _tmp36) +
_tmp36 * (P(0, 5) * _tmp51 + P(1, 5) * _tmp50 + P(2, 5) * _tmp49 +
P(3, 5) * _tmp33 + P(4, 5) * _tmp38 + P(5, 5) * _tmp36) +
_tmp38 * (P(0, 4) * _tmp51 + P(1, 4) * _tmp50 + P(2, 4) * _tmp49 +
P(3, 4) * _tmp33 + P(4, 4) * _tmp38 + P(5, 4) * _tmp36) +
_tmp49 * (P(0, 2) * _tmp51 + P(1, 2) * _tmp50 + P(2, 2) * _tmp49 +
P(3, 2) * _tmp33 + P(4, 2) * _tmp38 + P(5, 2) * _tmp36) +
_tmp50 * (P(0, 1) * _tmp51 + P(1, 1) * _tmp50 + P(2, 1) * _tmp49 +
P(3, 1) * _tmp33 + P(4, 1) * _tmp38 + P(5, 1) * _tmp36) +
_tmp51 * (P(0, 0) * _tmp51 + P(1, 0) * _tmp50 + P(2, 0) * _tmp49 +
P(3, 0) * _tmp33 + P(4, 0) * _tmp38 + P(5, 0) * _tmp36);
_innov_var(2, 0) = R(2, 0) +
_tmp57 * (P(0, 1) * _tmp62 + P(1, 1) * _tmp57 + P(2, 1) * _tmp61 +
P(3, 1) * _tmp59 + P(4, 1) * _tmp58 + P(5, 1) * _tmp60) +
_tmp58 * (P(0, 4) * _tmp62 + P(1, 4) * _tmp57 + P(2, 4) * _tmp61 +
P(3, 4) * _tmp59 + P(4, 4) * _tmp58 + P(5, 4) * _tmp60) +
_tmp59 * (P(0, 3) * _tmp62 + P(1, 3) * _tmp57 + P(2, 3) * _tmp61 +
P(3, 3) * _tmp59 + P(4, 3) * _tmp58 + P(5, 3) * _tmp60) +
_tmp60 * (P(0, 5) * _tmp62 + P(1, 5) * _tmp57 + P(2, 5) * _tmp61 +
P(3, 5) * _tmp59 + P(4, 5) * _tmp58 + P(5, 5) * _tmp60) +
_tmp61 * (P(0, 2) * _tmp62 + P(1, 2) * _tmp57 + P(2, 2) * _tmp61 +
P(3, 2) * _tmp59 + P(4, 2) * _tmp58 + P(5, 2) * _tmp60) +
_tmp62 * (P(0, 0) * _tmp62 + P(1, 0) * _tmp57 + P(2, 0) * _tmp61 +
P(3, 0) * _tmp59 + P(4, 0) * _tmp58 + P(5, 0) * _tmp60);
}
if (Hx != nullptr) {
matrix::Matrix<Scalar, 24, 1>& _hx = (*Hx);
_hx.setZero();
_hx(0, 0) = _tmp32;
_hx(1, 0) = _tmp20;
_hx(2, 0) = _tmp31;
_hx(3, 0) = _tmp30;
_hx(4, 0) = _tmp27;
_hx(5, 0) = _tmp24;
}
if (Hy != nullptr) {
matrix::Matrix<Scalar, 24, 1>& _hy = (*Hy);
_hy.setZero();
_hy(0, 0) = _tmp51;
_hy(1, 0) = _tmp50;
_hy(2, 0) = _tmp49;
_hy(3, 0) = _tmp33;
_hy(4, 0) = _tmp38;
_hy(5, 0) = _tmp36;
}
if (Hz != nullptr) {
matrix::Matrix<Scalar, 24, 1>& _hz = (*Hz);
_hz.setZero();
_hz(0, 0) = _tmp62;
_hz(1, 0) = _tmp57;
_hz(2, 0) = _tmp61;
_hz(3, 0) = _tmp59;
_hz(4, 0) = _tmp58;
_hz(5, 0) = _tmp60;
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym

81
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_body_vel_y_innov_var.h Normal file
View File

@ -0,0 +1,81 @@
// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
// function/FUNCTION.h.jinja
// Do NOT modify by hand.
// -----------------------------------------------------------------------------
#pragma once
#include <matrix/math.hpp>
namespace sym {
/**
* This function was autogenerated from a symbolic function. Do not modify by hand.
*
* Symbolic function: compute_body_vel_y_innov_var
*
* Args:
* state: Matrix25_1
* P: Matrix24_24
* R: Scalar
*
* Outputs:
* innov_var: Scalar
*/
template <typename Scalar>
void ComputeBodyVelYInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
Scalar* const innov_var = nullptr) {
// Total ops: 138
// Input arrays
// Intermediate terms (19)
const Scalar _tmp0 = 2 * state(3, 0);
const Scalar _tmp1 = 2 * state(1, 0);
const Scalar _tmp2 = -_tmp0 * state(0, 0) + _tmp1 * state(2, 0);
const Scalar _tmp3 = _tmp0 * state(2, 0) + _tmp1 * state(0, 0);
const Scalar _tmp4 =
-2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
const Scalar _tmp5 = 2 * state(4, 0);
const Scalar _tmp6 = _tmp1 * state(6, 0) - _tmp5 * state(3, 0);
const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp6;
const Scalar _tmp8 =
(Scalar(1) / Scalar(2)) * _tmp0 * state(6, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(4, 0);
const Scalar _tmp9 = 4 * state(5, 0);
const Scalar _tmp10 = 2 * state(6, 0);
const Scalar _tmp11 = _tmp10 * state(2, 0) - _tmp5 * state(0, 0) - _tmp9 * state(3, 0);
const Scalar _tmp12 = (Scalar(1) / Scalar(2)) * _tmp11;
const Scalar _tmp13 = _tmp10 * state(0, 0) + _tmp5 * state(2, 0) - _tmp9 * state(1, 0);
const Scalar _tmp14 = (Scalar(1) / Scalar(2)) * state(2, 0);
const Scalar _tmp15 =
_tmp12 * state(0, 0) - _tmp13 * _tmp14 - _tmp7 * state(3, 0) + _tmp8 * state(1, 0);
const Scalar _tmp16 = (Scalar(1) / Scalar(2)) * _tmp13;
const Scalar _tmp17 =
-_tmp12 * state(1, 0) - _tmp14 * _tmp6 + _tmp16 * state(3, 0) + _tmp8 * state(0, 0);
const Scalar _tmp18 =
_tmp11 * _tmp14 + _tmp16 * state(0, 0) - _tmp7 * state(1, 0) - _tmp8 * state(3, 0);
// Output terms (1)
if (innov_var != nullptr) {
Scalar& _innov_var = (*innov_var);
_innov_var = R +
_tmp15 * (P(0, 2) * _tmp18 + P(1, 2) * _tmp17 + P(2, 2) * _tmp15 +
P(3, 2) * _tmp2 + P(4, 2) * _tmp4 + P(5, 2) * _tmp3) +
_tmp17 * (P(0, 1) * _tmp18 + P(1, 1) * _tmp17 + P(2, 1) * _tmp15 +
P(3, 1) * _tmp2 + P(4, 1) * _tmp4 + P(5, 1) * _tmp3) +
_tmp18 * (P(0, 0) * _tmp18 + P(1, 0) * _tmp17 + P(2, 0) * _tmp15 +
P(3, 0) * _tmp2 + P(4, 0) * _tmp4 + P(5, 0) * _tmp3) +
_tmp2 * (P(0, 3) * _tmp18 + P(1, 3) * _tmp17 + P(2, 3) * _tmp15 + P(3, 3) * _tmp2 +
P(4, 3) * _tmp4 + P(5, 3) * _tmp3) +
_tmp3 * (P(0, 5) * _tmp18 + P(1, 5) * _tmp17 + P(2, 5) * _tmp15 + P(3, 5) * _tmp2 +
P(4, 5) * _tmp4 + P(5, 5) * _tmp3) +
_tmp4 * (P(0, 4) * _tmp18 + P(1, 4) * _tmp17 + P(2, 4) * _tmp15 + P(3, 4) * _tmp2 +
P(4, 4) * _tmp4 + P(5, 4) * _tmp3);
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym

82
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_body_vel_z_innov_var.h Normal file
View File

@ -0,0 +1,82 @@
// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
// function/FUNCTION.h.jinja
// Do NOT modify by hand.
// -----------------------------------------------------------------------------
#pragma once
#include <matrix/math.hpp>
namespace sym {
/**
* This function was autogenerated from a symbolic function. Do not modify by hand.
*
* Symbolic function: compute_body_vel_z_innov_var
*
* Args:
* state: Matrix25_1
* P: Matrix24_24
* R: Scalar
*
* Outputs:
* innov_var: Scalar
*/
template <typename Scalar>
void ComputeBodyVelZInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
Scalar* const innov_var = nullptr) {
// Total ops: 142
// Input arrays
// Intermediate terms (15)
const Scalar _tmp0 = 2 * state(4, 0);
const Scalar _tmp1 = 2 * state(5, 0);
const Scalar _tmp2 =
(Scalar(1) / Scalar(2)) * _tmp0 * state(1, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(2, 0);
const Scalar _tmp3 =
(Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) - Scalar(1) / Scalar(2) * _tmp1 * state(1, 0);
const Scalar _tmp4 = 2 * state(0, 0);
const Scalar _tmp5 = 4 * state(6, 0);
const Scalar _tmp6 = (Scalar(1) / Scalar(2)) * _tmp1 * state(3, 0) +
(Scalar(1) / Scalar(2)) * _tmp4 * state(4, 0) -
Scalar(1) / Scalar(2) * _tmp5 * state(2, 0);
const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp0 * state(3, 0) -
Scalar(1) / Scalar(2) * _tmp4 * state(5, 0) -
Scalar(1) / Scalar(2) * _tmp5 * state(1, 0);
const Scalar _tmp8 =
-_tmp2 * state(1, 0) - _tmp3 * state(2, 0) + _tmp6 * state(0, 0) + _tmp7 * state(3, 0);
const Scalar _tmp9 = 2 * state(3, 0);
const Scalar _tmp10 = -_tmp4 * state(1, 0) + _tmp9 * state(2, 0);
const Scalar _tmp11 = _tmp4 * state(2, 0) + _tmp9 * state(1, 0);
const Scalar _tmp12 =
-2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1;
const Scalar _tmp13 =
_tmp2 * state(0, 0) - _tmp3 * state(3, 0) + _tmp6 * state(1, 0) - _tmp7 * state(2, 0);
const Scalar _tmp14 =
_tmp2 * state(2, 0) - _tmp3 * state(1, 0) - _tmp6 * state(3, 0) + _tmp7 * state(0, 0);
// Output terms (1)
if (innov_var != nullptr) {
Scalar& _innov_var = (*innov_var);
_innov_var = R +
_tmp10 * (P(0, 4) * _tmp14 + P(1, 4) * _tmp8 + P(2, 4) * _tmp13 +
P(3, 4) * _tmp11 + P(4, 4) * _tmp10 + P(5, 4) * _tmp12) +
_tmp11 * (P(0, 3) * _tmp14 + P(1, 3) * _tmp8 + P(2, 3) * _tmp13 +
P(3, 3) * _tmp11 + P(4, 3) * _tmp10 + P(5, 3) * _tmp12) +
_tmp12 * (P(0, 5) * _tmp14 + P(1, 5) * _tmp8 + P(2, 5) * _tmp13 +
P(3, 5) * _tmp11 + P(4, 5) * _tmp10 + P(5, 5) * _tmp12) +
_tmp13 * (P(0, 2) * _tmp14 + P(1, 2) * _tmp8 + P(2, 2) * _tmp13 +
P(3, 2) * _tmp11 + P(4, 2) * _tmp10 + P(5, 2) * _tmp12) +
_tmp14 * (P(0, 0) * _tmp14 + P(1, 0) * _tmp8 + P(2, 0) * _tmp13 +
P(3, 0) * _tmp11 + P(4, 0) * _tmp10 + P(5, 0) * _tmp12) +
_tmp8 * (P(0, 1) * _tmp14 + P(1, 1) * _tmp8 + P(2, 1) * _tmp13 + P(3, 1) * _tmp11 +
P(4, 1) * _tmp10 + P(5, 1) * _tmp12);
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym

63
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hx.h
View File

@ -1,63 +0,0 @@
// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
// function/FUNCTION.h.jinja
// Do NOT modify by hand.
// -----------------------------------------------------------------------------
#pragma once
#include <matrix/math.hpp>
namespace sym {
/**
* This function was autogenerated from a symbolic function. Do not modify by hand.
*
* Symbolic function: compute_ev_body_vel_hx
*
* Args:
* state: Matrix25_1
*
* Outputs:
* H: Matrix24_1
*/
template <typename Scalar>
void ComputeEvBodyVelHx(const matrix::Matrix<Scalar, 25, 1>& state,
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
// Total ops: 60
// Input arrays
// Intermediate terms (13)
const Scalar _tmp0 = 2 * state(5, 0);
const Scalar _tmp1 = 2 * state(6, 0);
const Scalar _tmp2 = _tmp0 * state(3, 0) - _tmp1 * state(2, 0);
const Scalar _tmp3 = (Scalar(1) / Scalar(2)) * state(1, 0);
const Scalar _tmp4 =
(Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(3, 0);
const Scalar _tmp5 = 4 * state(4, 0);
const Scalar _tmp6 = 2 * state(1, 0);
const Scalar _tmp7 = _tmp0 * state(0, 0) - _tmp5 * state(3, 0) + _tmp6 * state(6, 0);
const Scalar _tmp8 = (Scalar(1) / Scalar(2)) * _tmp7;
const Scalar _tmp9 = 2 * state(0, 0);
const Scalar _tmp10 = _tmp0 * state(1, 0) - _tmp5 * state(2, 0) - _tmp9 * state(6, 0);
const Scalar _tmp11 = (Scalar(1) / Scalar(2)) * _tmp10;
const Scalar _tmp12 = (Scalar(1) / Scalar(2)) * _tmp2;
// Output terms (1)
if (H != nullptr) {
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
_h.setZero();
_h(0, 0) = -_tmp11 * state(3, 0) - _tmp2 * _tmp3 + _tmp4 * state(0, 0) + _tmp8 * state(2, 0);
_h(1, 0) = _tmp11 * state(0, 0) - _tmp12 * state(2, 0) - _tmp3 * _tmp7 + _tmp4 * state(3, 0);
_h(2, 0) = _tmp10 * _tmp3 - _tmp12 * state(3, 0) - _tmp4 * state(2, 0) + _tmp8 * state(0, 0);
_h(3, 0) = -2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
_h(4, 0) = _tmp6 * state(2, 0) + _tmp9 * state(3, 0);
_h(5, 0) = _tmp6 * state(3, 0) - _tmp9 * state(2, 0);
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym

67
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hy.h
View File

@ -1,67 +0,0 @@
// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
// function/FUNCTION.h.jinja
// Do NOT modify by hand.
// -----------------------------------------------------------------------------
#pragma once
#include <matrix/math.hpp>
namespace sym {
/**
* This function was autogenerated from a symbolic function. Do not modify by hand.
*
* Symbolic function: compute_ev_body_vel_hy
*
* Args:
* state: Matrix25_1
*
* Outputs:
* H: Matrix24_1
*/
template <typename Scalar>
void ComputeEvBodyVelHy(const matrix::Matrix<Scalar, 25, 1>& state,
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
// Total ops: 64
// Input arrays
// Intermediate terms (9)
const Scalar _tmp0 = 2 * state(4, 0);
const Scalar _tmp1 = 2 * state(1, 0);
const Scalar _tmp2 =
-Scalar(1) / Scalar(2) * _tmp0 * state(3, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(6, 0);
const Scalar _tmp3 = 2 * state(3, 0);
const Scalar _tmp4 =
(Scalar(1) / Scalar(2)) * _tmp0 * state(1, 0) + (Scalar(1) / Scalar(2)) * _tmp3 * state(6, 0);
const Scalar _tmp5 = 4 * state(5, 0);
const Scalar _tmp6 = 2 * state(6, 0);
const Scalar _tmp7 = -Scalar(1) / Scalar(2) * _tmp0 * state(0, 0) -
Scalar(1) / Scalar(2) * _tmp5 * state(3, 0) +
(Scalar(1) / Scalar(2)) * _tmp6 * state(2, 0);
const Scalar _tmp8 = (Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) -
Scalar(1) / Scalar(2) * _tmp5 * state(1, 0) +
(Scalar(1) / Scalar(2)) * _tmp6 * state(0, 0);
// Output terms (1)
if (H != nullptr) {
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
_h.setZero();
_h(0, 0) =
-_tmp2 * state(1, 0) - _tmp4 * state(3, 0) + _tmp7 * state(2, 0) + _tmp8 * state(0, 0);
_h(1, 0) =
-_tmp2 * state(2, 0) + _tmp4 * state(0, 0) - _tmp7 * state(1, 0) + _tmp8 * state(3, 0);
_h(2, 0) =
-_tmp2 * state(3, 0) + _tmp4 * state(1, 0) + _tmp7 * state(0, 0) - _tmp8 * state(2, 0);
_h(3, 0) = _tmp1 * state(2, 0) - _tmp3 * state(0, 0);
_h(4, 0) = -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
_h(5, 0) = _tmp1 * state(0, 0) + _tmp3 * state(2, 0);
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym

63
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hz.h
View File

@ -1,63 +0,0 @@
// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
// function/FUNCTION.h.jinja
// Do NOT modify by hand.
// -----------------------------------------------------------------------------
#pragma once
#include <matrix/math.hpp>
namespace sym {
/**
* This function was autogenerated from a symbolic function. Do not modify by hand.
*
* Symbolic function: compute_ev_body_vel_hz
*
* Args:
* state: Matrix25_1
*
* Outputs:
* H: Matrix24_1
*/
template <typename Scalar>
void ComputeEvBodyVelHz(const matrix::Matrix<Scalar, 25, 1>& state,
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
// Total ops: 60
// Input arrays
// Intermediate terms (13)
const Scalar _tmp0 = 2 * state(4, 0);
const Scalar _tmp1 = 2 * state(5, 0);
const Scalar _tmp2 =
(Scalar(1) / Scalar(2)) * _tmp0 * state(1, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(2, 0);
const Scalar _tmp3 = _tmp0 * state(2, 0) - _tmp1 * state(1, 0);
const Scalar _tmp4 = (Scalar(1) / Scalar(2)) * _tmp3;
const Scalar _tmp5 = 4 * state(6, 0);
const Scalar _tmp6 = _tmp0 * state(3, 0) - _tmp1 * state(0, 0) - _tmp5 * state(1, 0);
const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp6;
const Scalar _tmp8 = _tmp0 * state(0, 0) + _tmp1 * state(3, 0) - _tmp5 * state(2, 0);
const Scalar _tmp9 = (Scalar(1) / Scalar(2)) * state(3, 0);
const Scalar _tmp10 = (Scalar(1) / Scalar(2)) * _tmp8;
const Scalar _tmp11 = 2 * state(2, 0);
const Scalar _tmp12 = 2 * state(1, 0);
// Output terms (1)
if (H != nullptr) {
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
_h.setZero();
_h(0, 0) = _tmp2 * state(2, 0) - _tmp4 * state(1, 0) + _tmp7 * state(0, 0) - _tmp8 * _tmp9;
_h(1, 0) = _tmp10 * state(0, 0) - _tmp2 * state(1, 0) - _tmp4 * state(2, 0) + _tmp6 * _tmp9;
_h(2, 0) = _tmp10 * state(1, 0) + _tmp2 * state(0, 0) - _tmp3 * _tmp9 - _tmp7 * state(2, 0);
_h(3, 0) = _tmp11 * state(0, 0) + _tmp12 * state(3, 0);
_h(4, 0) = _tmp11 * state(3, 0) - _tmp12 * state(0, 0);
_h(5, 0) = -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1;
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym