mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-17 05:30:35 +08:00
ekf2: generate yaw fusion jacobians using symforce
This commit is contained in:
bresch
committed by
Mathieu Bresciani
Mathieu Bresciani
parent
ccd90ede7a
commit
f11908a266
@@ -253,6 +253,74 @@ def compute_mag_z_innov_var_and_h(
|
||||
|
||||
return (innov_var, H.T)
|
||||
|
||||
def compute_yaw_321_innov_var_and_h(
|
||||
state: VState,
|
||||
P: MState,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VState):
|
||||
|
||||
q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
|
||||
R_to_earth = quat_to_rot(q_att)
|
||||
# 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: MState,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VState):
|
||||
|
||||
q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
|
||||
R_to_earth = quat_to_rot(q_att)
|
||||
# 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: MState,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VState):
|
||||
|
||||
q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
|
||||
R_to_earth = quat_to_rot(q_att)
|
||||
# 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: MState,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VState):
|
||||
|
||||
q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
|
||||
R_to_earth = quat_to_rot(q_att)
|
||||
# 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)
|
||||
|
||||
print("Derive EKF2 equations...")
|
||||
generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
|
||||
generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"])
|
||||
@@ -263,3 +331,7 @@ generate_px4_function(predict_covariance, output_names=["P_new"])
|
||||
generate_px4_function(compute_mag_innov_innov_var_and_hx, output_names=["innov", "innov_var", "Hx"])
|
||||
generate_px4_function(compute_mag_y_innov_var_and_h, output_names=["innov_var", "H"])
|
||||
generate_px4_function(compute_mag_z_innov_var_and_h, 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_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"])
|
||||
|
||||
+75
@@ -0,0 +1,75 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// This file was autogenerated by symforce from template:
|
||||
// backends/cpp/templates/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: Matrix24_24
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeYaw312InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 75
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (11)
|
||||
const Scalar _tmp0 = -state(0, 0) * state(3, 0) + state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp1 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(1, 0), Scalar(2)) +
|
||||
std::pow(state(2, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp2 = _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp3 = std::pow(_tmp2, Scalar(2));
|
||||
const Scalar _tmp4 = 4 * _tmp0 / _tmp3;
|
||||
const Scalar _tmp5 = 2 / _tmp2;
|
||||
const Scalar _tmp6 = _tmp3 / (4 * std::pow(_tmp0, Scalar(2)) + _tmp3);
|
||||
const Scalar _tmp7 = _tmp6 * (-_tmp4 * state(1, 0) - _tmp5 * state(2, 0));
|
||||
const Scalar _tmp8 = _tmp6 * (_tmp4 * state(2, 0) - _tmp5 * state(1, 0));
|
||||
const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(3, 0) + _tmp5 * state(0, 0));
|
||||
const Scalar _tmp10 = _tmp6 * (_tmp4 * state(0, 0) + _tmp5 * state(3, 0));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R +
|
||||
_tmp10 * (P(0, 0) * _tmp10 + P(1, 0) * _tmp7 + P(2, 0) * _tmp8 + P(3, 0) * _tmp9) +
|
||||
_tmp7 * (P(0, 1) * _tmp10 + P(1, 1) * _tmp7 + P(2, 1) * _tmp8 + P(3, 1) * _tmp9) +
|
||||
_tmp8 * (P(0, 2) * _tmp10 + P(1, 2) * _tmp7 + P(2, 2) * _tmp8 + P(3, 2) * _tmp9) +
|
||||
_tmp9 * (P(0, 3) * _tmp10 + P(1, 3) * _tmp7 + P(2, 3) * _tmp8 + P(3, 3) * _tmp9);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
|
||||
_H.setZero();
|
||||
|
||||
_H(0, 0) = _tmp10;
|
||||
_H(1, 0) = _tmp7;
|
||||
_H(2, 0) = _tmp8;
|
||||
_H(3, 0) = _tmp9;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
+75
@@ -0,0 +1,75 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// This file was autogenerated by symforce from template:
|
||||
// backends/cpp/templates/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: Matrix24_24
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 83
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (11)
|
||||
const Scalar _tmp0 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(1, 0), Scalar(2)) +
|
||||
std::pow(state(2, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = -state(0, 0) * state(3, 0) + state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp2 = 2 * _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp3 = std::pow(_tmp2, Scalar(2));
|
||||
const Scalar _tmp4 = 2 * _tmp0 / _tmp3;
|
||||
const Scalar _tmp5 = 2 / _tmp2;
|
||||
const Scalar _tmp6 = _tmp3 / (std::pow(_tmp0, Scalar(2)) + _tmp3);
|
||||
const Scalar _tmp7 = _tmp6 * (_tmp4 * state(1, 0) - _tmp5 * state(2, 0));
|
||||
const Scalar _tmp8 = _tmp6 * (_tmp4 * state(2, 0) + _tmp5 * state(1, 0));
|
||||
const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(0, 0) + _tmp5 * state(3, 0));
|
||||
const Scalar _tmp10 = _tmp6 * (-_tmp4 * state(3, 0) - _tmp5 * state(0, 0));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var =
|
||||
R - _tmp10 * (-P(0, 0) * _tmp10 - P(1, 0) * _tmp8 - P(2, 0) * _tmp7 - P(3, 0) * _tmp9) -
|
||||
_tmp7 * (-P(0, 2) * _tmp10 - P(1, 2) * _tmp8 - P(2, 2) * _tmp7 - P(3, 2) * _tmp9) -
|
||||
_tmp8 * (-P(0, 1) * _tmp10 - P(1, 1) * _tmp8 - P(2, 1) * _tmp7 - P(3, 1) * _tmp9) -
|
||||
_tmp9 * (-P(0, 3) * _tmp10 - P(1, 3) * _tmp8 - P(2, 3) * _tmp7 - P(3, 3) * _tmp9);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
|
||||
_H.setZero();
|
||||
|
||||
_H(0, 0) = -_tmp10;
|
||||
_H(1, 0) = -_tmp8;
|
||||
_H(2, 0) = -_tmp7;
|
||||
_H(3, 0) = -_tmp9;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
+75
@@ -0,0 +1,75 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// This file was autogenerated by symforce from template:
|
||||
// backends/cpp/templates/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: Matrix24_24
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeYaw321InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 74
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (11)
|
||||
const Scalar _tmp0 = state(0, 0) * state(3, 0) + state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp1 = std::pow(state(0, 0), Scalar(2)) + std::pow(state(1, 0), Scalar(2)) -
|
||||
std::pow(state(2, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp2 = _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp3 = std::pow(_tmp2, Scalar(2));
|
||||
const Scalar _tmp4 = 4 * _tmp0 / _tmp3;
|
||||
const Scalar _tmp5 = 2 / _tmp2;
|
||||
const Scalar _tmp6 = _tmp3 / (4 * std::pow(_tmp0, Scalar(2)) + _tmp3);
|
||||
const Scalar _tmp7 = _tmp6 * (_tmp4 * state(2, 0) + _tmp5 * state(1, 0));
|
||||
const Scalar _tmp8 = _tmp6 * (-_tmp4 * state(1, 0) + _tmp5 * state(2, 0));
|
||||
const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(0, 0) + _tmp5 * state(3, 0));
|
||||
const Scalar _tmp10 = _tmp6 * (_tmp4 * state(3, 0) + _tmp5 * state(0, 0));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R +
|
||||
_tmp10 * (P(0, 3) * _tmp9 + P(1, 3) * _tmp8 + P(2, 3) * _tmp7 + P(3, 3) * _tmp10) +
|
||||
_tmp7 * (P(0, 2) * _tmp9 + P(1, 2) * _tmp8 + P(2, 2) * _tmp7 + P(3, 2) * _tmp10) +
|
||||
_tmp8 * (P(0, 1) * _tmp9 + P(1, 1) * _tmp8 + P(2, 1) * _tmp7 + P(3, 1) * _tmp10) +
|
||||
_tmp9 * (P(0, 0) * _tmp9 + P(1, 0) * _tmp8 + P(2, 0) * _tmp7 + P(3, 0) * _tmp10);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
|
||||
_H.setZero();
|
||||
|
||||
_H(0, 0) = _tmp9;
|
||||
_H(1, 0) = _tmp8;
|
||||
_H(2, 0) = _tmp7;
|
||||
_H(3, 0) = _tmp10;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
+75
@@ -0,0 +1,75 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// This file was autogenerated by symforce from template:
|
||||
// backends/cpp/templates/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: Matrix24_24
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 84
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (11)
|
||||
const Scalar _tmp0 = std::pow(state(0, 0), Scalar(2)) + std::pow(state(1, 0), Scalar(2)) -
|
||||
std::pow(state(2, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = state(0, 0) * state(3, 0) + state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp2 = 2 * _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp3 = std::pow(_tmp2, Scalar(2));
|
||||
const Scalar _tmp4 = 2 * _tmp0 / _tmp3;
|
||||
const Scalar _tmp5 = 2 / _tmp2;
|
||||
const Scalar _tmp6 = _tmp3 / (std::pow(_tmp0, Scalar(2)) + _tmp3);
|
||||
const Scalar _tmp7 = _tmp6 * (-_tmp4 * state(0, 0) - _tmp5 * state(3, 0));
|
||||
const Scalar _tmp8 = _tmp6 * (-_tmp4 * state(2, 0) + _tmp5 * state(1, 0));
|
||||
const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(3, 0) + _tmp5 * state(0, 0));
|
||||
const Scalar _tmp10 = _tmp6 * (-_tmp4 * state(1, 0) - _tmp5 * state(2, 0));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var =
|
||||
R - _tmp10 * (-P(0, 2) * _tmp9 - P(1, 2) * _tmp8 - P(2, 2) * _tmp10 - P(3, 2) * _tmp7) -
|
||||
_tmp7 * (-P(0, 3) * _tmp9 - P(1, 3) * _tmp8 - P(2, 3) * _tmp10 - P(3, 3) * _tmp7) -
|
||||
_tmp8 * (-P(0, 1) * _tmp9 - P(1, 1) * _tmp8 - P(2, 1) * _tmp10 - P(3, 1) * _tmp7) -
|
||||
_tmp9 * (-P(0, 0) * _tmp9 - P(1, 0) * _tmp8 - P(2, 0) * _tmp10 - P(3, 0) * _tmp7);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
|
||||
_H.setZero();
|
||||
|
||||
_H(0, 0) = -_tmp9;
|
||||
_H(1, 0) = -_tmp8;
|
||||
_H(2, 0) = -_tmp10;
|
||||
_H(3, 0) = -_tmp7;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
@@ -57,4 +57,5 @@ px4_add_unit_gtest(SRC test_EKF_terrain_estimator.cpp LINKLIBS ecl_EKF ecl_senso
|
||||
px4_add_unit_gtest(SRC test_EKF_utils.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
px4_add_unit_gtest(SRC test_EKF_withReplayData.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
px4_add_unit_gtest(SRC test_EKF_yaw_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
|
||||
px4_add_unit_gtest(SRC test_EKF_yaw_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
|
||||
px4_add_unit_gtest(SRC test_SensorRangeFinder.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
|
||||
@@ -0,0 +1,102 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include "EKF/ekf.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"
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
TEST(YawFusionGenerated, singularityYawEquivalence)
|
||||
{
|
||||
// GIVEN: an attitude that should give a singularity when transforming the
|
||||
// rotation matrix to Euler yaw
|
||||
const Quatf q(Eulerf(M_PI_F, 0.f, M_PI_F));
|
||||
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q(0);
|
||||
state_vector(1) = q(1);
|
||||
state_vector(2) = q(2);
|
||||
state_vector(3) = q(3);
|
||||
|
||||
const float R = sq(radians(10.f));
|
||||
SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
|
||||
Vector24f H_a;
|
||||
Vector24f H_b;
|
||||
float innov_var_a;
|
||||
float innov_var_b;
|
||||
|
||||
// WHEN: computing the innovation variance and H using two different
|
||||
// alternate forms (one is singular at pi/2 and the other one at 0)
|
||||
sym::ComputeYaw321InnovVarAndH(state_vector, P, R, FLT_EPSILON, &innov_var_a, &H_a);
|
||||
sym::ComputeYaw321InnovVarAndHAlternate(state_vector, P, R, FLT_EPSILON, &innov_var_b, &H_b);
|
||||
|
||||
// THEN: Even at the singularity point, the result is still correct, thanks to epsilon
|
||||
EXPECT_TRUE(isEqual(H_a, H_b));
|
||||
EXPECT_NEAR(innov_var_a, innov_var_b, 1e-5f);
|
||||
EXPECT_TRUE(innov_var_a < 5.f && innov_var_a > R) << "innov_var = " << innov_var_a;
|
||||
}
|
||||
|
||||
TEST(YawFusionGenerated, gimbalLock321vs312)
|
||||
{
|
||||
// GIVEN: an attitude at gimbal lock position
|
||||
const Quatf q(Eulerf(0.f, -M_PI_F / 2.f, M_PI_F));
|
||||
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q(0);
|
||||
state_vector(1) = q(1);
|
||||
state_vector(2) = q(2);
|
||||
state_vector(3) = q(3);
|
||||
|
||||
const float R = sq(radians(10.f));
|
||||
SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
|
||||
Vector24f H_321;
|
||||
Vector24f H_312;
|
||||
float innov_var_321;
|
||||
float innov_var_312;
|
||||
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);
|
||||
|
||||
// THEN: both computation are not equivalent, 321 is undefined but 312 is valid
|
||||
EXPECT_FALSE(isEqual(H_321, H_312));
|
||||
EXPECT_GT(fabsf(innov_var_321 - innov_var_312), 1e6f);
|
||||
EXPECT_TRUE(innov_var_312 < 5.f && innov_var_312 > R) << "innov_var = " << innov_var_312;
|
||||
}
|
||||
Reference in New Issue
Block a user