forked from Archive/PX4-Autopilot
ekf2: remove old yaw 321 and 312 derivations
This commit is contained in:
bresch
Daniel Agar
parent
ee63f3e664
commit
b2f1122372
|
|
@ -408,74 +408,6 @@ def compute_yaw_innov_var_and_h(
|
||||||
|
|
||||||
return (innov_var, H.T)
|
return (innov_var, H.T)
|
||||||
|
|
||||||
def compute_yaw_321_innov_var_and_h(
|
|
||||||
state: VState,
|
|
||||||
P: MTangent,
|
|
||||||
R: sf.Scalar,
|
|
||||||
epsilon: sf.Scalar
|
|
||||||
) -> (sf.Scalar, VTangent):
|
|
||||||
|
|
||||||
state = vstate_to_state(state)
|
|
||||||
R_to_earth = state["quat_nominal"].to_rotation_matrix()
|
|
||||||
# Fix the singularity at pi/2 by inserting epsilon
|
|
||||||
meas_pred = sf.atan2(R_to_earth[1,0], R_to_earth[0,0], epsilon=epsilon)
|
|
||||||
|
|
||||||
H = sf.V1(meas_pred).jacobian(state)
|
|
||||||
innov_var = (H * P * H.T + R)[0,0]
|
|
||||||
|
|
||||||
return (innov_var, H.T)
|
|
||||||
|
|
||||||
def compute_yaw_321_innov_var_and_h_alternate(
|
|
||||||
state: VState,
|
|
||||||
P: MTangent,
|
|
||||||
R: sf.Scalar,
|
|
||||||
epsilon: sf.Scalar
|
|
||||||
) -> (sf.Scalar, VTangent):
|
|
||||||
|
|
||||||
state = vstate_to_state(state)
|
|
||||||
R_to_earth = state["quat_nominal"].to_rotation_matrix()
|
|
||||||
# Alternate form that has a singularity at yaw 0 instead of pi/2
|
|
||||||
meas_pred = sf.pi/2 - sf.atan2(R_to_earth[0,0], R_to_earth[1,0], epsilon=epsilon)
|
|
||||||
|
|
||||||
H = sf.V1(meas_pred).jacobian(state)
|
|
||||||
innov_var = (H * P * H.T + R)[0,0]
|
|
||||||
|
|
||||||
return (innov_var, H.T)
|
|
||||||
|
|
||||||
def compute_yaw_312_innov_var_and_h(
|
|
||||||
state: VState,
|
|
||||||
P: MTangent,
|
|
||||||
R: sf.Scalar,
|
|
||||||
epsilon: sf.Scalar
|
|
||||||
) -> (sf.Scalar, VTangent):
|
|
||||||
|
|
||||||
state = vstate_to_state(state)
|
|
||||||
R_to_earth = state["quat_nominal"].to_rotation_matrix()
|
|
||||||
# Alternate form to be used when close to pitch +-pi/2
|
|
||||||
meas_pred = sf.atan2(-R_to_earth[0,1], R_to_earth[1,1], epsilon=epsilon)
|
|
||||||
|
|
||||||
H = sf.V1(meas_pred).jacobian(state)
|
|
||||||
innov_var = (H * P * H.T + R)[0,0]
|
|
||||||
|
|
||||||
return (innov_var, H.T)
|
|
||||||
|
|
||||||
def compute_yaw_312_innov_var_and_h_alternate(
|
|
||||||
state: VState,
|
|
||||||
P: MTangent,
|
|
||||||
R: sf.Scalar,
|
|
||||||
epsilon: sf.Scalar
|
|
||||||
) -> (sf.Scalar, VTangent):
|
|
||||||
|
|
||||||
state = vstate_to_state(state)
|
|
||||||
R_to_earth = state["quat_nominal"].to_rotation_matrix()
|
|
||||||
# Alternate form to be used when close to pitch +-pi/2
|
|
||||||
meas_pred = sf.pi/2 - sf.atan2(-R_to_earth[1,1], R_to_earth[0,1], epsilon=epsilon)
|
|
||||||
|
|
||||||
H = sf.V1(meas_pred).jacobian(state)
|
|
||||||
innov_var = (H * P * H.T + R)[0,0]
|
|
||||||
|
|
||||||
return (innov_var, H.T)
|
|
||||||
|
|
||||||
def compute_mag_declination_pred_innov_var_and_h(
|
def compute_mag_declination_pred_innov_var_and_h(
|
||||||
state: VState,
|
state: VState,
|
||||||
P: MTangent,
|
P: MTangent,
|
||||||
|
|
@ -697,10 +629,6 @@ if not args.disable_wind:
|
||||||
generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"])
|
generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"])
|
||||||
|
|
||||||
generate_px4_function(compute_yaw_innov_var_and_h, output_names=["innov_var", "H"])
|
generate_px4_function(compute_yaw_innov_var_and_h, output_names=["innov_var", "H"])
|
||||||
generate_px4_function(compute_yaw_312_innov_var_and_h, output_names=["innov_var", "H"])
|
|
||||||
generate_px4_function(compute_yaw_312_innov_var_and_h_alternate, output_names=["innov_var", "H"])
|
|
||||||
generate_px4_function(compute_yaw_321_innov_var_and_h, output_names=["innov_var", "H"])
|
|
||||||
generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=["innov_var", "H"])
|
|
||||||
generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
|
generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
|
||||||
generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])
|
generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])
|
||||||
generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "H"])
|
generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "H"])
|
||||||
|
|
|
||||||
|
|
@ -1,76 +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_yaw_312_innov_var_and_h
|
|
||||||
*
|
|
||||||
* Args:
|
|
||||||
* state: Matrix24_1
|
|
||||||
* P: Matrix23_23
|
|
||||||
* R: Scalar
|
|
||||||
* epsilon: Scalar
|
|
||||||
*
|
|
||||||
* Outputs:
|
|
||||||
* innov_var: Scalar
|
|
||||||
* H: Matrix23_1
|
|
||||||
*/
|
|
||||||
template <typename Scalar>
|
|
||||||
void ComputeYaw312InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
|
||||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
|
||||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
|
||||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
|
||||||
// Total ops: 53
|
|
||||||
|
|
||||||
// Input arrays
|
|
||||||
|
|
||||||
// Intermediate terms (15)
|
|
||||||
const Scalar _tmp0 = 2 * state(0, 0);
|
|
||||||
const Scalar _tmp1 = -_tmp0 * state(3, 0);
|
|
||||||
const Scalar _tmp2 = 2 * state(1, 0);
|
|
||||||
const Scalar _tmp3 = _tmp2 * state(2, 0);
|
|
||||||
const Scalar _tmp4 = _tmp1 + _tmp3;
|
|
||||||
const Scalar _tmp5 = std::pow(state(3, 0), Scalar(2));
|
|
||||||
const Scalar _tmp6 = std::pow(state(1, 0), Scalar(2));
|
|
||||||
const Scalar _tmp7 = -2 * _tmp5 - 2 * _tmp6 + 1;
|
|
||||||
const Scalar _tmp8 = _tmp7 + epsilon * ((((_tmp7) > 0) - ((_tmp7) < 0)) + Scalar(0.5));
|
|
||||||
const Scalar _tmp9 = std::pow(_tmp8, Scalar(2));
|
|
||||||
const Scalar _tmp10 = _tmp4 / _tmp9;
|
|
||||||
const Scalar _tmp11 = Scalar(1.0) / (_tmp8);
|
|
||||||
const Scalar _tmp12 = _tmp9 / (std::pow(_tmp4, Scalar(2)) + _tmp9);
|
|
||||||
const Scalar _tmp13 = _tmp12 * (_tmp10 * (_tmp1 - _tmp3) -
|
|
||||||
_tmp11 * (_tmp5 - _tmp6 - std::pow(state(0, 0), Scalar(2)) +
|
|
||||||
std::pow(state(2, 0), Scalar(2))));
|
|
||||||
const Scalar _tmp14 = _tmp12 * (_tmp10 * (-_tmp2 * state(0, 0) + 2 * state(2, 0) * state(3, 0)) -
|
|
||||||
_tmp11 * (_tmp0 * state(2, 0) + _tmp2 * state(3, 0)));
|
|
||||||
|
|
||||||
// Output terms (2)
|
|
||||||
if (innov_var != nullptr) {
|
|
||||||
Scalar& _innov_var = (*innov_var);
|
|
||||||
|
|
||||||
_innov_var = R + _tmp13 * (P(0, 2) * _tmp14 + P(2, 2) * _tmp13) +
|
|
||||||
_tmp14 * (P(0, 0) * _tmp14 + P(2, 0) * _tmp13);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (H != nullptr) {
|
|
||||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
|
||||||
|
|
||||||
_h.setZero();
|
|
||||||
|
|
||||||
_h(0, 0) = _tmp14;
|
|
||||||
_h(2, 0) = _tmp13;
|
|
||||||
}
|
|
||||||
} // NOLINT(readability/fn_size)
|
|
||||||
|
|
||||||
// NOLINTNEXTLINE(readability/fn_size)
|
|
||||||
} // namespace sym
|
|
||||||
|
|
@ -1,76 +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_yaw_312_innov_var_and_h_alternate
|
|
||||||
*
|
|
||||||
* Args:
|
|
||||||
* state: Matrix24_1
|
|
||||||
* P: Matrix23_23
|
|
||||||
* R: Scalar
|
|
||||||
* epsilon: Scalar
|
|
||||||
*
|
|
||||||
* Outputs:
|
|
||||||
* innov_var: Scalar
|
|
||||||
* H: Matrix23_1
|
|
||||||
*/
|
|
||||||
template <typename Scalar>
|
|
||||||
void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& state,
|
|
||||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
|
||||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
|
||||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
|
||||||
// Total ops: 57
|
|
||||||
|
|
||||||
// Input arrays
|
|
||||||
|
|
||||||
// Intermediate terms (15)
|
|
||||||
const Scalar _tmp0 = 2 * state(0, 0);
|
|
||||||
const Scalar _tmp1 = 2 * state(1, 0);
|
|
||||||
const Scalar _tmp2 = std::pow(state(3, 0), Scalar(2));
|
|
||||||
const Scalar _tmp3 = std::pow(state(1, 0), Scalar(2));
|
|
||||||
const Scalar _tmp4 = -2 * _tmp2 - 2 * _tmp3 + 1;
|
|
||||||
const Scalar _tmp5 = -_tmp0 * state(3, 0);
|
|
||||||
const Scalar _tmp6 = _tmp1 * state(2, 0);
|
|
||||||
const Scalar _tmp7 = _tmp5 + _tmp6;
|
|
||||||
const Scalar _tmp8 = _tmp7 + epsilon * ((((_tmp7) > 0) - ((_tmp7) < 0)) + Scalar(0.5));
|
|
||||||
const Scalar _tmp9 = std::pow(_tmp8, Scalar(2));
|
|
||||||
const Scalar _tmp10 = _tmp4 / _tmp9;
|
|
||||||
const Scalar _tmp11 = Scalar(1.0) / (_tmp8);
|
|
||||||
const Scalar _tmp12 = _tmp9 / (std::pow(_tmp4, Scalar(2)) + _tmp9);
|
|
||||||
const Scalar _tmp13 = _tmp12 * (_tmp10 * (_tmp0 * state(2, 0) + _tmp1 * state(3, 0)) -
|
|
||||||
_tmp11 * (-_tmp1 * state(0, 0) + 2 * state(2, 0) * state(3, 0)));
|
|
||||||
const Scalar _tmp14 = _tmp12 * (_tmp10 * (_tmp2 - _tmp3 - std::pow(state(0, 0), Scalar(2)) +
|
|
||||||
std::pow(state(2, 0), Scalar(2))) -
|
|
||||||
_tmp11 * (_tmp5 - _tmp6));
|
|
||||||
|
|
||||||
// Output terms (2)
|
|
||||||
if (innov_var != nullptr) {
|
|
||||||
Scalar& _innov_var = (*innov_var);
|
|
||||||
|
|
||||||
_innov_var = R - _tmp13 * (-P(0, 0) * _tmp13 - P(2, 0) * _tmp14) -
|
|
||||||
_tmp14 * (-P(0, 2) * _tmp13 - P(2, 2) * _tmp14);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (H != nullptr) {
|
|
||||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
|
||||||
|
|
||||||
_h.setZero();
|
|
||||||
|
|
||||||
_h(0, 0) = -_tmp13;
|
|
||||||
_h(2, 0) = -_tmp14;
|
|
||||||
}
|
|
||||||
} // NOLINT(readability/fn_size)
|
|
||||||
|
|
||||||
// NOLINTNEXTLINE(readability/fn_size)
|
|
||||||
} // namespace sym
|
|
||||||
|
|
@ -1,76 +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_yaw_321_innov_var_and_h
|
|
||||||
*
|
|
||||||
* Args:
|
|
||||||
* state: Matrix24_1
|
|
||||||
* P: Matrix23_23
|
|
||||||
* R: Scalar
|
|
||||||
* epsilon: Scalar
|
|
||||||
*
|
|
||||||
* Outputs:
|
|
||||||
* innov_var: Scalar
|
|
||||||
* H: Matrix23_1
|
|
||||||
*/
|
|
||||||
template <typename Scalar>
|
|
||||||
void ComputeYaw321InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
|
||||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
|
||||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
|
||||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
|
||||||
// Total ops: 53
|
|
||||||
|
|
||||||
// Input arrays
|
|
||||||
|
|
||||||
// Intermediate terms (15)
|
|
||||||
const Scalar _tmp0 = 2 * state(0, 0);
|
|
||||||
const Scalar _tmp1 = _tmp0 * state(3, 0);
|
|
||||||
const Scalar _tmp2 = 2 * state(1, 0);
|
|
||||||
const Scalar _tmp3 = _tmp2 * state(2, 0);
|
|
||||||
const Scalar _tmp4 = _tmp1 + _tmp3;
|
|
||||||
const Scalar _tmp5 = std::pow(state(3, 0), Scalar(2));
|
|
||||||
const Scalar _tmp6 = std::pow(state(2, 0), Scalar(2));
|
|
||||||
const Scalar _tmp7 = -2 * _tmp5 - 2 * _tmp6 + 1;
|
|
||||||
const Scalar _tmp8 = _tmp7 + epsilon * ((((_tmp7) > 0) - ((_tmp7) < 0)) + Scalar(0.5));
|
|
||||||
const Scalar _tmp9 = std::pow(_tmp8, Scalar(2));
|
|
||||||
const Scalar _tmp10 = _tmp4 / _tmp9;
|
|
||||||
const Scalar _tmp11 = Scalar(1.0) / (_tmp8);
|
|
||||||
const Scalar _tmp12 = _tmp9 / (std::pow(_tmp4, Scalar(2)) + _tmp9);
|
|
||||||
const Scalar _tmp13 = _tmp12 * (-_tmp10 * (-_tmp1 + _tmp3) +
|
|
||||||
_tmp11 * (-_tmp5 + _tmp6 + std::pow(state(0, 0), Scalar(2)) -
|
|
||||||
std::pow(state(1, 0), Scalar(2))));
|
|
||||||
const Scalar _tmp14 = _tmp12 * (-_tmp10 * (-_tmp0 * state(2, 0) - _tmp2 * state(3, 0)) +
|
|
||||||
_tmp11 * (_tmp2 * state(0, 0) - 2 * state(2, 0) * state(3, 0)));
|
|
||||||
|
|
||||||
// Output terms (2)
|
|
||||||
if (innov_var != nullptr) {
|
|
||||||
Scalar& _innov_var = (*innov_var);
|
|
||||||
|
|
||||||
_innov_var = R + _tmp13 * (P(1, 2) * _tmp14 + P(2, 2) * _tmp13) +
|
|
||||||
_tmp14 * (P(1, 1) * _tmp14 + P(2, 1) * _tmp13);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (H != nullptr) {
|
|
||||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
|
||||||
|
|
||||||
_h.setZero();
|
|
||||||
|
|
||||||
_h(1, 0) = _tmp14;
|
|
||||||
_h(2, 0) = _tmp13;
|
|
||||||
}
|
|
||||||
} // NOLINT(readability/fn_size)
|
|
||||||
|
|
||||||
// NOLINTNEXTLINE(readability/fn_size)
|
|
||||||
} // namespace sym
|
|
||||||
|
|
@ -1,76 +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_yaw_321_innov_var_and_h_alternate
|
|
||||||
*
|
|
||||||
* Args:
|
|
||||||
* state: Matrix24_1
|
|
||||||
* P: Matrix23_23
|
|
||||||
* R: Scalar
|
|
||||||
* epsilon: Scalar
|
|
||||||
*
|
|
||||||
* Outputs:
|
|
||||||
* innov_var: Scalar
|
|
||||||
* H: Matrix23_1
|
|
||||||
*/
|
|
||||||
template <typename Scalar>
|
|
||||||
void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& state,
|
|
||||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
|
||||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
|
||||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
|
||||||
// Total ops: 57
|
|
||||||
|
|
||||||
// Input arrays
|
|
||||||
|
|
||||||
// Intermediate terms (15)
|
|
||||||
const Scalar _tmp0 = 2 * state(2, 0);
|
|
||||||
const Scalar _tmp1 = 2 * state(1, 0);
|
|
||||||
const Scalar _tmp2 = 2 * state(0, 0) * state(3, 0);
|
|
||||||
const Scalar _tmp3 = _tmp1 * state(2, 0);
|
|
||||||
const Scalar _tmp4 = _tmp2 + _tmp3;
|
|
||||||
const Scalar _tmp5 = _tmp4 + epsilon * ((((_tmp4) > 0) - ((_tmp4) < 0)) + Scalar(0.5));
|
|
||||||
const Scalar _tmp6 = Scalar(1.0) / (_tmp5);
|
|
||||||
const Scalar _tmp7 = std::pow(state(3, 0), Scalar(2));
|
|
||||||
const Scalar _tmp8 = std::pow(state(2, 0), Scalar(2));
|
|
||||||
const Scalar _tmp9 = -2 * _tmp7 - 2 * _tmp8 + 1;
|
|
||||||
const Scalar _tmp10 = std::pow(_tmp5, Scalar(2));
|
|
||||||
const Scalar _tmp11 = _tmp9 / _tmp10;
|
|
||||||
const Scalar _tmp12 = _tmp10 / (_tmp10 + std::pow(_tmp9, Scalar(2)));
|
|
||||||
const Scalar _tmp13 = _tmp12 * (-_tmp11 * (-_tmp0 * state(3, 0) + _tmp1 * state(0, 0)) +
|
|
||||||
_tmp6 * (-_tmp0 * state(0, 0) - _tmp1 * state(3, 0)));
|
|
||||||
const Scalar _tmp14 = _tmp12 * (-_tmp11 * (-_tmp7 + _tmp8 + std::pow(state(0, 0), Scalar(2)) -
|
|
||||||
std::pow(state(1, 0), Scalar(2))) +
|
|
||||||
_tmp6 * (-_tmp2 + _tmp3));
|
|
||||||
|
|
||||||
// Output terms (2)
|
|
||||||
if (innov_var != nullptr) {
|
|
||||||
Scalar& _innov_var = (*innov_var);
|
|
||||||
|
|
||||||
_innov_var = R - _tmp13 * (-P(1, 1) * _tmp13 - P(2, 1) * _tmp14) -
|
|
||||||
_tmp14 * (-P(1, 2) * _tmp13 - P(2, 2) * _tmp14);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (H != nullptr) {
|
|
||||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
|
||||||
|
|
||||||
_h.setZero();
|
|
||||||
|
|
||||||
_h(1, 0) = -_tmp13;
|
|
||||||
_h(2, 0) = -_tmp14;
|
|
||||||
}
|
|
||||||
} // NOLINT(readability/fn_size)
|
|
||||||
|
|
||||||
// NOLINTNEXTLINE(readability/fn_size)
|
|
||||||
} // namespace sym
|
|
||||||
|
|
@ -33,8 +33,6 @@
|
||||||
|
|
||||||
#include "ekf.h"
|
#include "ekf.h"
|
||||||
|
|
||||||
#include <ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h>
|
|
||||||
#include <ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h>
|
|
||||||
#include <ekf_derivation/generated/compute_yaw_innov_var_and_h.h>
|
#include <ekf_derivation/generated/compute_yaw_innov_var_and_h.h>
|
||||||
|
|
||||||
#include <mathlib/mathlib.h>
|
#include <mathlib/mathlib.h>
|
||||||
|
|
|
||||||
|
|
@ -35,68 +35,10 @@
|
||||||
#include "EKF/ekf.h"
|
#include "EKF/ekf.h"
|
||||||
#include "test_helper/comparison_helper.h"
|
#include "test_helper/comparison_helper.h"
|
||||||
|
|
||||||
#include "../EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h"
|
|
||||||
#include "../EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h"
|
|
||||||
#include "../EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h"
|
|
||||||
#include "../EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h"
|
|
||||||
#include "../EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h"
|
#include "../EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h"
|
||||||
|
|
||||||
using namespace matrix;
|
using namespace matrix;
|
||||||
|
|
||||||
TEST(YawFusionGenerated, yawSingularity)
|
|
||||||
{
|
|
||||||
// GIVEN: an attitude that should give a singularity when transforming the
|
|
||||||
// rotation matrix to Euler yaw
|
|
||||||
StateSample state{};
|
|
||||||
state.quat_nominal = Eulerf(M_PI_F, 0.f, M_PI_F);
|
|
||||||
|
|
||||||
const float R = sq(radians(10.f));
|
|
||||||
SquareMatrixState P = createRandomCovarianceMatrix();
|
|
||||||
|
|
||||||
VectorState H_a;
|
|
||||||
VectorState H_b;
|
|
||||||
float innov_var_a;
|
|
||||||
float innov_var_b;
|
|
||||||
|
|
||||||
// WHEN: computing the innovation variance and H using two different methods
|
|
||||||
sym::ComputeYaw321InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var_a, &H_a);
|
|
||||||
sym::ComputeYawInnovVarAndH(state.vector(), P, R, &innov_var_b, &H_b);
|
|
||||||
|
|
||||||
// THEN: Even at the singularity point, the result is still correct
|
|
||||||
EXPECT_TRUE(isEqual(H_a, H_b));
|
|
||||||
|
|
||||||
EXPECT_NEAR(innov_var_a, innov_var_b, 1e-5f);
|
|
||||||
EXPECT_TRUE(innov_var_a < 50.f && innov_var_a > R) << "innov_var = " << innov_var_a;
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(YawFusionGenerated, gimbalLock321vs312vsTangent)
|
|
||||||
{
|
|
||||||
// GIVEN: an attitude at gimbal lock position
|
|
||||||
StateSample state{};
|
|
||||||
state.quat_nominal = Eulerf(0.f, -M_PI_F / 2.f, M_PI_F);
|
|
||||||
|
|
||||||
const float R = sq(radians(10.f));
|
|
||||||
SquareMatrixState P = createRandomCovarianceMatrix();
|
|
||||||
|
|
||||||
VectorState H_321;
|
|
||||||
VectorState H_312;
|
|
||||||
VectorState H_tangent;
|
|
||||||
float innov_var_321;
|
|
||||||
float innov_var_312;
|
|
||||||
float innov_var_tangent;
|
|
||||||
sym::ComputeYaw321InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var_321, &H_321);
|
|
||||||
|
|
||||||
sym::ComputeYaw312InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var_312, &H_312);
|
|
||||||
sym::ComputeYawInnovVarAndH(state.vector(), P, R, &innov_var_tangent, &H_tangent);
|
|
||||||
|
|
||||||
// THEN: both computation are not equivalent, 321 is undefined but 312 and "tangent" are valid
|
|
||||||
EXPECT_FALSE(isEqual(H_321, H_312));
|
|
||||||
EXPECT_TRUE(isEqual(H_312, H_tangent));
|
|
||||||
EXPECT_GT(fabsf(innov_var_321 - innov_var_312), 1e6f);
|
|
||||||
EXPECT_NEAR(innov_var_312, innov_var_tangent, 1e-6f);
|
|
||||||
EXPECT_TRUE(innov_var_312 < 50.f && innov_var_312 > R) << "innov_var = " << innov_var_312;
|
|
||||||
}
|
|
||||||
|
|
||||||
Vector3f getRotVarNed(const Quatf &q, const SquareMatrixState &P)
|
Vector3f getRotVarNed(const Quatf &q, const SquareMatrixState &P)
|
||||||
{
|
{
|
||||||
constexpr auto S = State::quat_nominal;
|
constexpr auto S = State::quat_nominal;
|
||||||
|
|
@ -138,6 +80,8 @@ TEST(YawFusionGenerated, positiveVarianceAllOrientations)
|
||||||
<< " roll = " << degrees(roll)
|
<< " roll = " << degrees(roll)
|
||||||
<< " innov_var = " << innov_var
|
<< " innov_var = " << innov_var
|
||||||
<< " innov_var_true = " << innov_var_true;
|
<< " innov_var_true = " << innov_var_true;
|
||||||
|
|
||||||
|
EXPECT_TRUE(H.isAllFinite());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue