mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 16:58:54 +08:00
ekf2: calcRotVecVariances using SymForce
This commit is contained in:
bresch
@@ -40,6 +40,7 @@
|
||||
*/
|
||||
|
||||
#include "ekf.h"
|
||||
#include "python/ekf_derivation/generated/quat_var_to_rot_var.h"
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <cstdlib>
|
||||
@@ -882,46 +883,9 @@ void Ekf::updateVerticalDeadReckoningStatus()
|
||||
// calculate the variances for the rotation vector equivalent
|
||||
Vector3f Ekf::calcRotVecVariances() const
|
||||
{
|
||||
Vector3f rot_var_vec;
|
||||
float q0, q1, q2, q3;
|
||||
|
||||
if (_state.quat_nominal(0) >= 0.0f) {
|
||||
q0 = _state.quat_nominal(0);
|
||||
q1 = _state.quat_nominal(1);
|
||||
q2 = _state.quat_nominal(2);
|
||||
q3 = _state.quat_nominal(3);
|
||||
|
||||
} else {
|
||||
q0 = -_state.quat_nominal(0);
|
||||
q1 = -_state.quat_nominal(1);
|
||||
q2 = -_state.quat_nominal(2);
|
||||
q3 = -_state.quat_nominal(3);
|
||||
}
|
||||
float t2 = q0*q0;
|
||||
float t3 = acosf(q0);
|
||||
float t4 = -t2+1.0f;
|
||||
float t5 = t2-1.0f;
|
||||
if ((t4 > 1e-9f) && (t5 < -1e-9f)) {
|
||||
float t6 = 1.0f/t5;
|
||||
float t7 = q1*t6*2.0f;
|
||||
float t8 = 1.0f/powf(t4,1.5f);
|
||||
float t9 = q0*q1*t3*t8*2.0f;
|
||||
float t10 = t7+t9;
|
||||
float t11 = 1.0f/sqrtf(t4);
|
||||
float t12 = q2*t6*2.0f;
|
||||
float t13 = q0*q2*t3*t8*2.0f;
|
||||
float t14 = t12+t13;
|
||||
float t15 = q3*t6*2.0f;
|
||||
float t16 = q0*q3*t3*t8*2.0f;
|
||||
float t17 = t15+t16;
|
||||
rot_var_vec(0) = t10*(P(0,0)*t10+P(1,0)*t3*t11*2.0f)+t3*t11*(P(0,1)*t10+P(1,1)*t3*t11*2.0f)*2.0f;
|
||||
rot_var_vec(1) = t14*(P(0,0)*t14+P(2,0)*t3*t11*2.0f)+t3*t11*(P(0,2)*t14+P(2,2)*t3*t11*2.0f)*2.0f;
|
||||
rot_var_vec(2) = t17*(P(0,0)*t17+P(3,0)*t3*t11*2.0f)+t3*t11*(P(0,3)*t17+P(3,3)*t3*t11*2.0f)*2.0f;
|
||||
} else {
|
||||
rot_var_vec = 4.0f * P.slice<3,3>(1,1).diag();
|
||||
}
|
||||
|
||||
return rot_var_vec;
|
||||
Vector3f rot_var;
|
||||
sym::QuatVarToRotVar(getStateAtFusionHorizonAsVector(), P, FLT_EPSILON, &rot_var);
|
||||
return rot_var;
|
||||
}
|
||||
|
||||
// initialise the quaternion covariances using rotation vector variances
|
||||
|
||||
@@ -502,6 +502,15 @@ def compute_gravity_innov_var_and_k_and_h(
|
||||
|
||||
return (innov, innov_var, K[0], K[1], K[2])
|
||||
|
||||
def quat_var_to_rot_var(
|
||||
state: VState,
|
||||
P: MState,
|
||||
epsilon: sf.Scalar
|
||||
):
|
||||
J = sf.V3(state_to_rot3(state).to_tangent(epsilon=epsilon)).jacobian(state)
|
||||
rot_cov = J * P * J.T
|
||||
return sf.V3(rot_cov[0, 0], rot_cov[1, 1], rot_cov[2, 2])
|
||||
|
||||
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"])
|
||||
@@ -523,3 +532,4 @@ generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas
|
||||
generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"])
|
||||
generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"])
|
||||
generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"])
|
||||
generate_px4_function(quat_var_to_rot_var, output_names=["rot_var"])
|
||||
|
||||
@@ -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: quat_var_to_rot_var
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* rot_var: Matrix31
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void QuatVarToRotVar(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar epsilon,
|
||||
matrix::Matrix<Scalar, 3, 1>* const rot_var = nullptr) {
|
||||
// Total ops: 61
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (17)
|
||||
const Scalar _tmp0 = std::fabs(state(0, 0));
|
||||
const Scalar _tmp1 = 1 - epsilon;
|
||||
const Scalar _tmp2 = math::min<Scalar>(_tmp0, _tmp1);
|
||||
const Scalar _tmp3 = 1 - std::pow(_tmp2, Scalar(2));
|
||||
const Scalar _tmp4 = (((state(0, 0)) > 0) - ((state(0, 0)) < 0));
|
||||
const Scalar _tmp5 = 2 * math::min<Scalar>(0, _tmp4) + 1;
|
||||
const Scalar _tmp6 = _tmp5 * std::acos(_tmp2);
|
||||
const Scalar _tmp7 = 2 * _tmp6 / std::sqrt(_tmp3);
|
||||
const Scalar _tmp8 = _tmp4 * ((((-_tmp0 + _tmp1) > 0) - ((-_tmp0 + _tmp1) < 0)) + 1);
|
||||
const Scalar _tmp9 = _tmp8 * state(1, 0);
|
||||
const Scalar _tmp10 = _tmp2 * _tmp6 / (_tmp3 * std::sqrt(_tmp3));
|
||||
const Scalar _tmp11 = _tmp5 / _tmp3;
|
||||
const Scalar _tmp12 = _tmp10 * _tmp9 - _tmp11 * _tmp9;
|
||||
const Scalar _tmp13 = _tmp10 * _tmp8;
|
||||
const Scalar _tmp14 = _tmp11 * _tmp8;
|
||||
const Scalar _tmp15 = _tmp13 * state(2, 0) - _tmp14 * state(2, 0);
|
||||
const Scalar _tmp16 = _tmp13 * state(3, 0) - _tmp14 * state(3, 0);
|
||||
|
||||
// Output terms (1)
|
||||
if (rot_var != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _rot_var = (*rot_var);
|
||||
|
||||
_rot_var(0, 0) = _tmp12 * (P(0, 0) * _tmp12 + P(1, 0) * _tmp7) +
|
||||
_tmp7 * (P(0, 1) * _tmp12 + P(1, 1) * _tmp7);
|
||||
_rot_var(1, 0) = _tmp15 * (P(0, 0) * _tmp15 + P(2, 0) * _tmp7) +
|
||||
_tmp7 * (P(0, 2) * _tmp15 + P(2, 2) * _tmp7);
|
||||
_rot_var(2, 0) = _tmp16 * (P(0, 0) * _tmp16 + P(3, 0) * _tmp7) +
|
||||
_tmp7 * (P(0, 3) * _tmp16 + P(3, 3) * _tmp7);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
@@ -37,6 +37,7 @@ add_subdirectory(sensor_simulator)
|
||||
add_subdirectory(test_helper)
|
||||
|
||||
px4_add_unit_gtest(SRC test_EKF_accelerometer.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
px4_add_unit_gtest(SRC test_EKF_attitude_variance.cpp LINKLIBS ecl_EKF ecl_test_helper)
|
||||
px4_add_unit_gtest(SRC test_EKF_airspeed_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
|
||||
px4_add_unit_gtest(SRC test_EKF_airspeed.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
px4_add_unit_gtest(SRC test_EKF_basics.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
|
||||
@@ -0,0 +1,132 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2023 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/quat_var_to_rot_var.h"
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
Vector3f calcRotVarMatlab(const Quatf &q, const SquareMatrix24f &P)
|
||||
{
|
||||
Vector3f rot_var_vec;
|
||||
float q0, q1, q2, q3;
|
||||
|
||||
if (q(0) >= 0.0f) {
|
||||
q0 = q(0);
|
||||
q1 = q(1);
|
||||
q2 = q(2);
|
||||
q3 = q(3);
|
||||
|
||||
} else {
|
||||
q0 = -q(0);
|
||||
q1 = -q(1);
|
||||
q2 = -q(2);
|
||||
q3 = -q(3);
|
||||
}
|
||||
|
||||
float t2 = q0 * q0;
|
||||
float t3 = acosf(q0);
|
||||
float t4 = -t2 + 1.0f;
|
||||
float t5 = t2 - 1.0f;
|
||||
|
||||
if ((t4 > 1e-9f) && (t5 < -1e-9f)) {
|
||||
float t6 = 1.0f / t5;
|
||||
float t7 = q1 * t6 * 2.0f;
|
||||
float t8 = 1.0f / powf(t4, 1.5f);
|
||||
float t9 = q0 * q1 * t3 * t8 * 2.0f;
|
||||
float t10 = t7 + t9;
|
||||
float t11 = 1.0f / sqrtf(t4);
|
||||
float t12 = q2 * t6 * 2.0f;
|
||||
float t13 = q0 * q2 * t3 * t8 * 2.0f;
|
||||
float t14 = t12 + t13;
|
||||
float t15 = q3 * t6 * 2.0f;
|
||||
float t16 = q0 * q3 * t3 * t8 * 2.0f;
|
||||
float t17 = t15 + t16;
|
||||
rot_var_vec(0) = t10 * (P(0, 0) * t10 + P(1, 0) * t3 * t11 * 2.0f) + t3 * t11 * (P(0, 1) * t10 + P(1,
|
||||
1) * t3 * t11 * 2.0f) * 2.0f;
|
||||
rot_var_vec(1) = t14 * (P(0, 0) * t14 + P(2, 0) * t3 * t11 * 2.0f) + t3 * t11 * (P(0, 2) * t14 + P(2,
|
||||
2) * t3 * t11 * 2.0f) * 2.0f;
|
||||
rot_var_vec(2) = t17 * (P(0, 0) * t17 + P(3, 0) * t3 * t11 * 2.0f) + t3 * t11 * (P(0, 3) * t17 + P(3,
|
||||
3) * t3 * t11 * 2.0f) * 2.0f;
|
||||
|
||||
} else {
|
||||
rot_var_vec = 4.0f * P.slice<3, 3>(1, 1).diag();
|
||||
}
|
||||
|
||||
return rot_var_vec;
|
||||
}
|
||||
|
||||
TEST(AttitudeVariance, matlabVsSymforce)
|
||||
{
|
||||
Quatf q(Eulerf(M_PI_F / 4.f, -M_PI_F / 6.f, M_PI_F));
|
||||
q = -q; // use non-canonical quaternion to cover special case
|
||||
|
||||
const SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
Vector3f rot_var_matlab = calcRotVarMatlab(q, P);
|
||||
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q(0);
|
||||
state_vector(1) = q(1);
|
||||
state_vector(2) = q(2);
|
||||
state_vector(3) = q(3);
|
||||
|
||||
Vector3f rot_var_symforce;
|
||||
sym::QuatVarToRotVar(state_vector, P, FLT_EPSILON, &rot_var_symforce);
|
||||
|
||||
EXPECT_EQ(rot_var_matlab, rot_var_symforce);
|
||||
}
|
||||
|
||||
TEST(AttitudeVariance, matlabVsSymforceZeroTilt)
|
||||
{
|
||||
Quatf q;
|
||||
|
||||
const SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
Vector3f rot_var_matlab = calcRotVarMatlab(q, P);
|
||||
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q(0);
|
||||
state_vector(1) = q(1);
|
||||
state_vector(2) = q(2);
|
||||
state_vector(3) = q(3);
|
||||
|
||||
Vector3f rot_var_symforce;
|
||||
sym::QuatVarToRotVar(state_vector, P, FLT_EPSILON, &rot_var_symforce);
|
||||
|
||||
EXPECT_EQ(rot_var_matlab, rot_var_symforce);
|
||||
|
||||
const Vector3f rot_var_true = 4.0f * P.slice<3, 3>(1, 1).diag(); // special case
|
||||
EXPECT_EQ(rot_var_symforce, rot_var_true);
|
||||
}
|
||||
Reference in New Issue
Block a user