mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-28 05:40:36 +08:00
ekf2: compute quat tilt variance using SymForce
This commit is contained in:
bresch
@@ -511,21 +511,25 @@ def quat_var_to_rot_var(
|
||||
rot_cov = J * P * J.T
|
||||
return sf.V3(rot_cov[0, 0], rot_cov[1, 1], rot_cov[2, 2])
|
||||
|
||||
def yaw_var_to_lower_triangular_quat_cov(
|
||||
def rot_var_ned_to_lower_triangular_quat_cov(
|
||||
state: VState,
|
||||
yaw_var: sf.Scalar
|
||||
rot_var_ned: sf.V3
|
||||
):
|
||||
# This function converts an attitude variance defined by a 3D vector in NED frame
|
||||
# into a 4x4 covariance matrix representing the uncertainty on each of the 4 quaternion parameters
|
||||
# Note: the resulting quaternion uncertainty is defined as a perturbation
|
||||
# at the tip of the quaternion (i.e.:body-frame uncertainty)
|
||||
q = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
|
||||
attitude = state_to_rot3(state)
|
||||
J = q.jacobian(attitude)
|
||||
|
||||
# Convert yaw uncertainty from NED to body frame
|
||||
yaw_cov_ned = sf.M33.diag([0, 0, yaw_var])
|
||||
# Convert uncertainties from NED to body frame
|
||||
rot_cov_ned = sf.M33.diag(rot_var_ned)
|
||||
adjoint = attitude.to_rotation_matrix() # the adjoint of SO(3) is simply the rotation matrix itself
|
||||
yaw_cov_body = adjoint.T * yaw_cov_ned * adjoint
|
||||
rot_cov_body = adjoint.T * rot_cov_ned * adjoint
|
||||
|
||||
# Convert yaw (body) to quaternion parameter uncertainty
|
||||
q_var = J * yaw_cov_body * J.T
|
||||
q_var = J * rot_cov_body * J.T
|
||||
|
||||
# Generate lower trangle only and copy it to the upper part in implementation (produces less code)
|
||||
return q_var.lower_triangle()
|
||||
@@ -552,4 +556,4 @@ generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var",
|
||||
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"])
|
||||
generate_px4_function(yaw_var_to_lower_triangular_quat_cov, output_names=["q_cov_lower_triangle"])
|
||||
generate_px4_function(rot_var_ned_to_lower_triangular_quat_cov, output_names=["q_cov_lower_triangle"])
|
||||
|
||||
+123
@@ -0,0 +1,123 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// 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: rot_var_ned_to_lower_triangular_quat_cov
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* rot_var_ned: Matrix31
|
||||
*
|
||||
* Outputs:
|
||||
* q_cov_lower_triangle: Matrix44
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void RotVarNedToLowerTriangularQuatCov(
|
||||
const matrix::Matrix<Scalar, 24, 1>& state, const matrix::Matrix<Scalar, 3, 1>& rot_var_ned,
|
||||
matrix::Matrix<Scalar, 4, 4>* const q_cov_lower_triangle = nullptr) {
|
||||
// Total ops: 185
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (54)
|
||||
const Scalar _tmp0 = 2 * state(0, 0) * state(2, 0);
|
||||
const Scalar _tmp1 = 2 * state(3, 0);
|
||||
const Scalar _tmp2 = _tmp1 * state(1, 0);
|
||||
const Scalar _tmp3 = _tmp0 + _tmp2;
|
||||
const Scalar _tmp4 = _tmp1 * state(0, 0);
|
||||
const Scalar _tmp5 = 2 * state(1, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(2, 0);
|
||||
const Scalar _tmp7 = -_tmp4 + _tmp6;
|
||||
const Scalar _tmp8 = -2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp9 = -2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp10 = _tmp8 + _tmp9 + 1;
|
||||
const Scalar _tmp11 = _tmp1 * state(2, 0);
|
||||
const Scalar _tmp12 = _tmp5 * state(0, 0);
|
||||
const Scalar _tmp13 = _tmp11 - _tmp12;
|
||||
const Scalar _tmp14 = _tmp13 * rot_var_ned(1, 0);
|
||||
const Scalar _tmp15 = 1 - 2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp16 = _tmp15 + _tmp9;
|
||||
const Scalar _tmp17 = _tmp11 + _tmp12;
|
||||
const Scalar _tmp18 = _tmp17 * rot_var_ned(2, 0);
|
||||
const Scalar _tmp19 = _tmp10 * _tmp14 + _tmp16 * _tmp18 + _tmp3 * _tmp7 * rot_var_ned(0, 0);
|
||||
const Scalar _tmp20 = (Scalar(1) / Scalar(2)) * state(3, 0);
|
||||
const Scalar _tmp21 = -_tmp19 * _tmp20;
|
||||
const Scalar _tmp22 = std::pow(_tmp10, Scalar(2)) * rot_var_ned(1, 0) +
|
||||
std::pow(_tmp17, Scalar(2)) * rot_var_ned(2, 0) +
|
||||
std::pow(_tmp7, Scalar(2)) * rot_var_ned(0, 0);
|
||||
const Scalar _tmp23 = (Scalar(1) / Scalar(2)) * state(2, 0);
|
||||
const Scalar _tmp24 = _tmp15 + _tmp8;
|
||||
const Scalar _tmp25 = _tmp24 * rot_var_ned(0, 0);
|
||||
const Scalar _tmp26 = _tmp4 + _tmp6;
|
||||
const Scalar _tmp27 = -_tmp0 + _tmp2;
|
||||
const Scalar _tmp28 = (Scalar(1) / Scalar(2)) * _tmp10 * _tmp26 * rot_var_ned(1, 0) +
|
||||
(Scalar(1) / Scalar(2)) * _tmp18 * _tmp27 +
|
||||
(Scalar(1) / Scalar(2)) * _tmp25 * _tmp7;
|
||||
const Scalar _tmp29 = _tmp28 * state(1, 0);
|
||||
const Scalar _tmp30 = std::pow(_tmp24, Scalar(2)) * rot_var_ned(0, 0) +
|
||||
std::pow(_tmp26, Scalar(2)) * rot_var_ned(1, 0) +
|
||||
std::pow(_tmp27, Scalar(2)) * rot_var_ned(2, 0);
|
||||
const Scalar _tmp31 = (Scalar(1) / Scalar(2)) * state(1, 0);
|
||||
const Scalar _tmp32 = _tmp14 * _tmp26 + _tmp16 * _tmp27 * rot_var_ned(2, 0) + _tmp25 * _tmp3;
|
||||
const Scalar _tmp33 = _tmp20 * _tmp32;
|
||||
const Scalar _tmp34 = -_tmp28 * state(2, 0);
|
||||
const Scalar _tmp35 = _tmp19 * _tmp23;
|
||||
const Scalar _tmp36 = std::pow(_tmp13, Scalar(2)) * rot_var_ned(1, 0) +
|
||||
std::pow(_tmp16, Scalar(2)) * rot_var_ned(2, 0) +
|
||||
std::pow(_tmp3, Scalar(2)) * rot_var_ned(0, 0);
|
||||
const Scalar _tmp37 = -_tmp31 * _tmp32;
|
||||
const Scalar _tmp38 = _tmp28 * state(0, 0);
|
||||
const Scalar _tmp39 = -_tmp20 * _tmp22 + _tmp35 + _tmp38;
|
||||
const Scalar _tmp40 = (Scalar(1) / Scalar(2)) * state(0, 0);
|
||||
const Scalar _tmp41 = _tmp23 * _tmp32;
|
||||
const Scalar _tmp42 = _tmp28 * state(3, 0);
|
||||
const Scalar _tmp43 = _tmp30 * _tmp40 + _tmp41 - _tmp42;
|
||||
const Scalar _tmp44 = _tmp32 * _tmp40;
|
||||
const Scalar _tmp45 = _tmp21 + _tmp23 * _tmp36 + _tmp44;
|
||||
const Scalar _tmp46 = _tmp19 * _tmp31;
|
||||
const Scalar _tmp47 = _tmp22 * _tmp40 + _tmp42 - _tmp46;
|
||||
const Scalar _tmp48 = _tmp20 * _tmp30 + _tmp37 + _tmp38;
|
||||
const Scalar _tmp49 = _tmp19 * _tmp40;
|
||||
const Scalar _tmp50 = -_tmp31 * _tmp36 + _tmp33 + _tmp49;
|
||||
const Scalar _tmp51 = _tmp22 * _tmp31 + _tmp34 + _tmp49;
|
||||
const Scalar _tmp52 = -_tmp23 * _tmp30 + _tmp29 + _tmp44;
|
||||
const Scalar _tmp53 = _tmp36 * _tmp40 - _tmp41 + _tmp46;
|
||||
|
||||
// Output terms (1)
|
||||
if (q_cov_lower_triangle != nullptr) {
|
||||
matrix::Matrix<Scalar, 4, 4>& _q_cov_lower_triangle = (*q_cov_lower_triangle);
|
||||
|
||||
_q_cov_lower_triangle(0, 0) = -_tmp20 * (-_tmp20 * _tmp36 - _tmp35 + _tmp37) -
|
||||
_tmp23 * (_tmp21 - _tmp22 * _tmp23 - _tmp29) -
|
||||
_tmp31 * (-_tmp30 * _tmp31 - _tmp33 + _tmp34);
|
||||
_q_cov_lower_triangle(1, 0) = -_tmp20 * _tmp45 - _tmp23 * _tmp39 - _tmp31 * _tmp43;
|
||||
_q_cov_lower_triangle(2, 0) = -_tmp20 * _tmp50 - _tmp23 * _tmp47 - _tmp31 * _tmp48;
|
||||
_q_cov_lower_triangle(3, 0) = -_tmp20 * _tmp53 - _tmp23 * _tmp51 - _tmp31 * _tmp52;
|
||||
_q_cov_lower_triangle(0, 1) = 0;
|
||||
_q_cov_lower_triangle(1, 1) = -_tmp20 * _tmp39 + _tmp23 * _tmp45 + _tmp40 * _tmp43;
|
||||
_q_cov_lower_triangle(2, 1) = -_tmp20 * _tmp47 + _tmp23 * _tmp50 + _tmp40 * _tmp48;
|
||||
_q_cov_lower_triangle(3, 1) = -_tmp20 * _tmp51 + _tmp23 * _tmp53 + _tmp40 * _tmp52;
|
||||
_q_cov_lower_triangle(0, 2) = 0;
|
||||
_q_cov_lower_triangle(1, 2) = 0;
|
||||
_q_cov_lower_triangle(2, 2) = _tmp20 * _tmp48 - _tmp31 * _tmp50 + _tmp40 * _tmp47;
|
||||
_q_cov_lower_triangle(3, 2) = _tmp20 * _tmp52 - _tmp31 * _tmp53 + _tmp40 * _tmp51;
|
||||
_q_cov_lower_triangle(0, 3) = 0;
|
||||
_q_cov_lower_triangle(1, 3) = 0;
|
||||
_q_cov_lower_triangle(2, 3) = 0;
|
||||
_q_cov_lower_triangle(3, 3) = -_tmp23 * _tmp52 + _tmp31 * _tmp51 + _tmp40 * _tmp53;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
-101
@@ -1,101 +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: yaw_var_to_lower_triangular_quat_cov
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* yaw_var: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* q_cov_lower_triangle: Matrix44
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void YawVarToLowerTriangularQuatCov(
|
||||
const matrix::Matrix<Scalar, 24, 1>& state, const Scalar yaw_var,
|
||||
matrix::Matrix<Scalar, 4, 4>* const q_cov_lower_triangle = nullptr) {
|
||||
// Total ops: 136
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (39)
|
||||
const Scalar _tmp0 = (Scalar(1) / Scalar(2)) * state(3, 0);
|
||||
const Scalar _tmp1 =
|
||||
-2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1;
|
||||
const Scalar _tmp2 = std::pow(_tmp1, Scalar(2)) * yaw_var;
|
||||
const Scalar _tmp3 = (Scalar(1) / Scalar(2)) * state(1, 0);
|
||||
const Scalar _tmp4 = 2 * state(2, 0);
|
||||
const Scalar _tmp5 = 2 * state(1, 0);
|
||||
const Scalar _tmp6 = -_tmp4 * state(0, 0) + _tmp5 * state(3, 0);
|
||||
const Scalar _tmp7 = _tmp1 * _tmp6 * yaw_var;
|
||||
const Scalar _tmp8 = -_tmp3 * _tmp7;
|
||||
const Scalar _tmp9 = (Scalar(1) / Scalar(2)) * state(2, 0);
|
||||
const Scalar _tmp10 = _tmp4 * state(3, 0) + _tmp5 * state(0, 0);
|
||||
const Scalar _tmp11 = _tmp10 * yaw_var;
|
||||
const Scalar _tmp12 = _tmp1 * _tmp11;
|
||||
const Scalar _tmp13 = _tmp12 * _tmp9;
|
||||
const Scalar _tmp14 = std::pow(_tmp10, Scalar(2)) * yaw_var;
|
||||
const Scalar _tmp15 = _tmp11 * _tmp6;
|
||||
const Scalar _tmp16 = _tmp15 * _tmp3;
|
||||
const Scalar _tmp17 = -_tmp0 * _tmp12;
|
||||
const Scalar _tmp18 = std::pow(_tmp6, Scalar(2)) * yaw_var;
|
||||
const Scalar _tmp19 = _tmp0 * _tmp7;
|
||||
const Scalar _tmp20 = -_tmp15 * _tmp9;
|
||||
const Scalar _tmp21 = (Scalar(1) / Scalar(2)) * state(0, 0);
|
||||
const Scalar _tmp22 = _tmp21 * _tmp7;
|
||||
const Scalar _tmp23 = _tmp17 + _tmp2 * _tmp9 + _tmp22;
|
||||
const Scalar _tmp24 = _tmp15 * _tmp21;
|
||||
const Scalar _tmp25 = -_tmp0 * _tmp14 + _tmp13 + _tmp24;
|
||||
const Scalar _tmp26 = _tmp7 * _tmp9;
|
||||
const Scalar _tmp27 = _tmp0 * _tmp15;
|
||||
const Scalar _tmp28 = _tmp18 * _tmp21 + _tmp26 - _tmp27;
|
||||
const Scalar _tmp29 = _tmp12 * _tmp21;
|
||||
const Scalar _tmp30 = _tmp19 - _tmp2 * _tmp3 + _tmp29;
|
||||
const Scalar _tmp31 = _tmp12 * _tmp3;
|
||||
const Scalar _tmp32 = _tmp14 * _tmp21 + _tmp27 - _tmp31;
|
||||
const Scalar _tmp33 = _tmp0 * _tmp18 + _tmp24 + _tmp8;
|
||||
const Scalar _tmp34 = _tmp2 * _tmp21 - _tmp26 + _tmp31;
|
||||
const Scalar _tmp35 = _tmp14 * _tmp3 + _tmp20 + _tmp29;
|
||||
const Scalar _tmp36 = (Scalar(1) / Scalar(2)) * _tmp35;
|
||||
const Scalar _tmp37 = _tmp16 - _tmp18 * _tmp9 + _tmp22;
|
||||
const Scalar _tmp38 = (Scalar(1) / Scalar(2)) * _tmp34;
|
||||
|
||||
// Output terms (1)
|
||||
if (q_cov_lower_triangle != nullptr) {
|
||||
matrix::Matrix<Scalar, 4, 4>& _q_cov_lower_triangle = (*q_cov_lower_triangle);
|
||||
|
||||
_q_cov_lower_triangle(0, 0) = -_tmp0 * (-_tmp0 * _tmp2 - _tmp13 + _tmp8) -
|
||||
_tmp3 * (-_tmp18 * _tmp3 - _tmp19 + _tmp20) -
|
||||
_tmp9 * (-_tmp14 * _tmp9 - _tmp16 + _tmp17);
|
||||
_q_cov_lower_triangle(1, 0) = -_tmp0 * _tmp23 - _tmp25 * _tmp9 - _tmp28 * _tmp3;
|
||||
_q_cov_lower_triangle(2, 0) = -_tmp0 * _tmp30 - _tmp3 * _tmp33 - _tmp32 * _tmp9;
|
||||
_q_cov_lower_triangle(3, 0) = -_tmp0 * _tmp34 - _tmp3 * _tmp37 - _tmp36 * state(2, 0);
|
||||
_q_cov_lower_triangle(0, 1) = 0;
|
||||
_q_cov_lower_triangle(1, 1) = -_tmp0 * _tmp25 + _tmp21 * _tmp28 + _tmp23 * _tmp9;
|
||||
_q_cov_lower_triangle(2, 1) = -_tmp0 * _tmp32 + _tmp21 * _tmp33 + _tmp30 * _tmp9;
|
||||
_q_cov_lower_triangle(3, 1) = -_tmp0 * _tmp35 + _tmp21 * _tmp37 + _tmp34 * _tmp9;
|
||||
_q_cov_lower_triangle(0, 2) = 0;
|
||||
_q_cov_lower_triangle(1, 2) = 0;
|
||||
_q_cov_lower_triangle(2, 2) = _tmp0 * _tmp33 + _tmp21 * _tmp32 - _tmp3 * _tmp30;
|
||||
_q_cov_lower_triangle(3, 2) = _tmp0 * _tmp37 + _tmp36 * state(0, 0) - _tmp38 * state(1, 0);
|
||||
_q_cov_lower_triangle(0, 3) = 0;
|
||||
_q_cov_lower_triangle(1, 3) = 0;
|
||||
_q_cov_lower_triangle(2, 3) = 0;
|
||||
_q_cov_lower_triangle(3, 3) = _tmp36 * state(1, 0) - _tmp37 * _tmp9 + _tmp38 * state(0, 0);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
Reference in New Issue
Block a user