mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-27 18:27:05 +08:00
ekf2: EV fusion in body frame (#23191)
This commit is contained in:
Marco Hauswirth
GitHub
@@ -7,6 +7,7 @@
|
||||
#
|
||||
# @maintainer
|
||||
# @board px4_fmu-v2 exclude
|
||||
# @board px4_fmu-v6x exclude
|
||||
#
|
||||
|
||||
. ${R}etc/init.d/rc.mc_defaults
|
||||
|
||||
@@ -13,6 +13,7 @@
|
||||
# @board px4_fmu-v4pro exclude
|
||||
# @board px4_fmu-v5 exclude
|
||||
# @board px4_fmu-v5x exclude
|
||||
# @board px4_fmu-v6x exclude
|
||||
# @board diatone_mamba-f405-mk2 exclude
|
||||
#
|
||||
. ${R}etc/init.d/rc.mc_defaults
|
||||
|
||||
@@ -37,6 +37,9 @@
|
||||
*/
|
||||
|
||||
#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>
|
||||
|
||||
void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
|
||||
const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src)
|
||||
@@ -57,14 +60,16 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
|
||||
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
|
||||
|
||||
// rotate measurement into correct earth frame if required
|
||||
Vector3f vel{NAN, NAN, NAN};
|
||||
Matrix3f vel_cov{};
|
||||
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) {
|
||||
vel = ev_sample.vel - vel_offset_earth;
|
||||
vel_cov = matrix::diag(ev_sample.velocity_var);
|
||||
measurement = ev_sample.vel - vel_offset_earth;
|
||||
measurement_var = ev_sample.velocity_var;
|
||||
|
||||
} else {
|
||||
continuing_conditions_passing = false;
|
||||
@@ -75,31 +80,28 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
|
||||
case VelocityFrame::LOCAL_FRAME_FRD:
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
// using EV frame
|
||||
vel = ev_sample.vel - vel_offset_earth;
|
||||
vel_cov = matrix::diag(ev_sample.velocity_var);
|
||||
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());
|
||||
|
||||
vel = R_ev_to_ekf * ev_sample.vel - vel_offset_earth;
|
||||
vel_cov = R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) * R_ev_to_ekf.transpose();
|
||||
|
||||
// increase minimum variance to include EV orientation variance
|
||||
// TODO: do this properly
|
||||
const float orientation_var_max = ev_sample.orientation_var.max();
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
vel_cov(i, i) = math::max(vel_cov(i, i), orientation_var_max);
|
||||
}
|
||||
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:
|
||||
vel = _R_to_earth * (ev_sample.vel - vel_offset_body);
|
||||
vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose();
|
||||
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;
|
||||
@@ -111,48 +113,56 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
|
||||
// increase minimum variance if GPS active (position reference)
|
||||
if (_control_status.flags.gps) {
|
||||
for (int i = 0; i < 2; i++) {
|
||||
vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise));
|
||||
measurement_var(i) = math::max(measurement_var(i), sq(_params.gps_vel_noise));
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
const Vector3f measurement{vel};
|
||||
|
||||
const Vector3f measurement_var{
|
||||
math::max(vel_cov(0, 0), sq(_params.ev_vel_noise), sq(0.01f)),
|
||||
math::max(vel_cov(1, 1), sq(_params.ev_vel_noise), sq(0.01f)),
|
||||
math::max(vel_cov(2, 2), sq(_params.ev_vel_noise), sq(0.01f))
|
||||
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();
|
||||
|
||||
const bool measurement_valid = 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);
|
||||
|
||||
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
|
||||
|
||||
if (!measurement_valid) {
|
||||
continuing_conditions_passing = false;
|
||||
} 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
|
||||
}
|
||||
|
||||
|
||||
const bool starting_conditions_passing = common_starting_conditions_passing
|
||||
&& continuing_conditions_passing
|
||||
&& ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive());
|
||||
|
||||
if (_control_status.flags.ev_vel) {
|
||||
|
||||
if (continuing_conditions_passing) {
|
||||
|
||||
if ((ev_reset && isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.ev_vel)) || yaw_alignment_changed) {
|
||||
|
||||
if (quality_sufficient) {
|
||||
ECL_INFO("reset to %s", AID_SRC_NAME);
|
||||
_information_events.flags.reset_vel_to_vision = true;
|
||||
resetVelocityTo(measurement, measurement_var);
|
||||
resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame);
|
||||
resetAidSourceStatusZeroInnovation(aid_src);
|
||||
|
||||
} else {
|
||||
@@ -163,7 +173,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
|
||||
}
|
||||
|
||||
} else if (quality_sufficient) {
|
||||
fuseVelocity(aid_src);
|
||||
fuseEvVelocity(aid_src, ev_sample);
|
||||
|
||||
} else {
|
||||
aid_src.innovation_rejected = true;
|
||||
@@ -177,24 +187,27 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
|
||||
// Data seems good, attempt a reset
|
||||
_information_events.flags.reset_vel_to_vision = true;
|
||||
ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
|
||||
resetVelocityTo(measurement, measurement_var);
|
||||
resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame);
|
||||
resetAidSourceStatusZeroInnovation(aid_src);
|
||||
|
||||
if (_control_status.flags.in_air) {
|
||||
_nb_ev_vel_reset_available--;
|
||||
}
|
||||
|
||||
} else 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);
|
||||
stopEvVelFusion();
|
||||
|
||||
} 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);
|
||||
// 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);
|
||||
|
||||
} 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();
|
||||
}
|
||||
}
|
||||
@@ -211,15 +224,13 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
|
||||
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));
|
||||
|
||||
_information_events.flags.reset_vel_to_vision = true;
|
||||
|
||||
resetVelocityTo(measurement, measurement_var);
|
||||
resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame);
|
||||
resetAidSourceStatusZeroInnovation(aid_src);
|
||||
|
||||
_control_status.flags.ev_vel = true;
|
||||
|
||||
} else if (fuseVelocity(aid_src)) {
|
||||
} else if (fuseEvVelocity(aid_src, ev_sample)) {
|
||||
ECL_INFO("starting %s fusion", AID_SRC_NAME);
|
||||
_control_status.flags.ev_vel = true;
|
||||
}
|
||||
@@ -232,6 +243,63 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::fuseEvVelocity(estimator_aid_source3d_s &aid_src, const extVisionSample &ev_sample)
|
||||
{
|
||||
if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) {
|
||||
|
||||
VectorState H;
|
||||
estimator_aid_source1d_s current_aid_src;
|
||||
const auto state_vector = _state.vector();
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
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) {
|
||||
@@ -239,3 +307,23 @@ void Ekf::stopEvVelFusion()
|
||||
_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();
|
||||
}
|
||||
|
||||
@@ -921,6 +921,8 @@ private:
|
||||
void controlEvPosFusion(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 extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src);
|
||||
void controlEvYawFusion(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 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, bool reset, estimator_aid_source2d_s &aid_src);
|
||||
@@ -928,6 +930,12 @@ private:
|
||||
void stopEvHgtFusion();
|
||||
void stopEvVelFusion();
|
||||
void stopEvYawFusion();
|
||||
bool fuseEvVelocity(estimator_aid_source3d_s &aid_src, const extVisionSample &ev_sample);
|
||||
void fuseBodyVelocity(estimator_aid_source1d_s &aid_src, float &innov_var, VectorState &H)
|
||||
{
|
||||
VectorState Kfusion = P * H / innov_var;
|
||||
aid_src.fused = measurementUpdate(Kfusion, H, aid_src.observation_variance, aid_src.innovation);
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
|
||||
@@ -362,6 +362,40 @@ def compute_sideslip_h_and_k(
|
||||
|
||||
return (H.T, K)
|
||||
|
||||
def predict_vel_body(
|
||||
state: VState
|
||||
) -> (sf.V3):
|
||||
vel = state["vel"]
|
||||
R_to_body = state["quat_nominal"].inverse()
|
||||
return R_to_body * vel
|
||||
|
||||
def compute_ev_body_vel_hx(
|
||||
state: VState,
|
||||
) -> (VTangent):
|
||||
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_vel_body(state)
|
||||
Hx = jacobian_chain_rule(meas_pred[0], state)
|
||||
return (Hx.T)
|
||||
|
||||
def compute_ev_body_vel_hy(
|
||||
state: VState,
|
||||
) -> (VTangent):
|
||||
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_vel_body(state)[1]
|
||||
Hy = jacobian_chain_rule(meas_pred, state)
|
||||
return (Hy.T)
|
||||
|
||||
def compute_ev_body_vel_hz(
|
||||
state: VState,
|
||||
) -> (VTangent):
|
||||
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_vel_body(state)[2]
|
||||
Hz = jacobian_chain_rule(meas_pred, state)
|
||||
return (Hz.T)
|
||||
|
||||
def predict_mag_body(state) -> sf.V3:
|
||||
mag_field_earth = state["mag_I"]
|
||||
mag_bias_body = state["mag_B"]
|
||||
@@ -697,5 +731,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_state(State, tangent_idx)
|
||||
|
||||
@@ -0,0 +1,63 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// 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
|
||||
@@ -0,0 +1,67 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// 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
|
||||
@@ -0,0 +1,63 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// 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
|
||||
@@ -259,37 +259,24 @@ TEST_F(EkfExternalVisionTest, visionAlignment)
|
||||
|
||||
TEST_F(EkfExternalVisionTest, velocityFrameBody)
|
||||
{
|
||||
// GIVEN: Drone is turned 90 degrees
|
||||
const Quatf quat_sim(Eulerf(0.0f, 0.0f, math::radians(90.0f)));
|
||||
_sensor_simulator.simulateOrientation(quat_sim);
|
||||
_ekf_wrapper.setMagFuseTypeNone();
|
||||
_sensor_simulator.runSeconds(_tilt_align_time);
|
||||
|
||||
_ekf->set_vehicle_at_rest(false);
|
||||
|
||||
// Without any measurement x and y velocity variance are close
|
||||
const Vector3f velVar_init = _ekf->getVelocityVariance();
|
||||
EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001);
|
||||
|
||||
// WHEN: measurement is given in BODY-FRAME and
|
||||
// x variance is bigger than y variance
|
||||
_sensor_simulator._vio.setVelocityFrameToBody();
|
||||
float yaw_var0 = _ekf->getYawVar();
|
||||
|
||||
const Vector3f vel_cov_body(2.0f, 0.01f, 0.01f);
|
||||
const Vector3f vel_body(1.0f, 0.0f, 0.0f);
|
||||
_sensor_simulator._vio.setVelocityFrameToBody();
|
||||
_sensor_simulator._vio.setVelocityVariance(vel_cov_body);
|
||||
_sensor_simulator._vio.setVelocity(vel_body);
|
||||
_ekf_wrapper.enableExternalVisionVelocityFusion();
|
||||
_sensor_simulator.startExternalVision();
|
||||
_sensor_simulator.runSeconds(4);
|
||||
|
||||
// THEN: As the drone is turned 90 degrees, velocity variance
|
||||
// along local y axis is expected to be bigger
|
||||
const Vector3f velVar_new = _ekf->getVelocityVariance();
|
||||
EXPECT_NEAR(velVar_new(1) / velVar_new(0), 30.f, 15.f);
|
||||
|
||||
const Vector3f vel_earth_est = _ekf->getVelocity();
|
||||
EXPECT_NEAR(vel_earth_est(0), 0.0f, 0.1f);
|
||||
EXPECT_NEAR(vel_earth_est(1), 1.0f, 0.1f);
|
||||
const Vector3f vel_var = _ekf->getVelocityVariance();
|
||||
EXPECT_TRUE(yaw_var0 < _ekf->getYawVar());
|
||||
EXPECT_TRUE(vel_var(1) < vel_var(0));
|
||||
}
|
||||
|
||||
TEST_F(EkfExternalVisionTest, velocityFrameLocal)
|
||||
|
||||
Reference in New Issue
Block a user