fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 03:20:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: migrate flow fusion to SymForce

Browse Source
This commit is contained in:
bresch
2022-10-25 18:05:28 +02:00
committed by Daniel Agar
parent 639d1ddca2
commit b54a4417fa
4 changed files with 288 additions and 168 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
+52
View File
@@ -337,6 +337,56 @@ def compute_mag_declination_innov_innov_var_and_h(
return (innov, innov_var, H.T)
def predict_opt_flow(state, distance, epsilon):
q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
R_to_earth = quat_to_rot(q_att)
R_to_body = R_to_earth.T
# Calculate earth relative velocity in a non-rotating sensor frame
v = sf.V3(state[State.vx], state[State.vy], state[State.vz])
rel_vel_sensor = R_to_body * v
# Divide by range to get predicted angular LOS rates relative to X and Y
# axes. Note these are rates in a non-rotating sensor frame
flow_pred = sf.V2()
flow_pred[0] = rel_vel_sensor[1] / distance
flow_pred[1] = -rel_vel_sensor[0] / distance
flow_pred = add_epsilon_sign(flow_pred, distance, epsilon)
return flow_pred
def compute_flow_xy_innov_var_and_hx(
state: VState,
P: MState,
distance: sf.Scalar,
R: sf.Scalar,
epsilon: sf.Scalar
) -> (sf.V2, VState):
meas_pred = predict_opt_flow(state, distance, epsilon);
innov_var = sf.V2()
Hx = sf.V1(meas_pred[0]).jacobian(state)
innov_var[0] = (Hx * P * Hx.T + R)[0,0]
Hy = sf.V1(meas_pred[1]).jacobian(state)
innov_var[1] = (Hy * P * Hy.T + R)[0,0]
return (innov_var, Hx.T)
def compute_flow_y_innov_var_and_h(
state: VState,
P: MState,
distance: sf.Scalar,
R: sf.Scalar,
epsilon: sf.Scalar
) -> (sf.Scalar, VState):
meas_pred = predict_opt_flow(state, distance, epsilon);
Hy = sf.V1(meas_pred[1]).jacobian(state)
innov_var = (Hy * P * Hy.T + R)[0,0]
return (innov_var, Hy.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"])
@@ -352,3 +402,5 @@ generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=["
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_mag_declination_innov_innov_var_and_h, output_names=["innov", "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"])
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h
+127
View File
@@ -0,0 +1,127 @@
// -----------------------------------------------------------------------------
// 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_flow_xy_innov_var_and_hx
*
* Args:
* state: Matrix24_1
* P: Matrix24_24
* distance: Scalar
* R: Scalar
* epsilon: Scalar
*
* Outputs:
* innov_var: Matrix21
* H: Matrix24_1
*/
template <typename Scalar>
void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar distance,
const Scalar R, const Scalar epsilon,
matrix::Matrix<Scalar, 2, 1>* const innov_var = nullptr,
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
// Total ops: 285
// Input arrays
// Intermediate terms (29)
const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
const Scalar _tmp3 =
Scalar(1.0) /
(distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
const Scalar _tmp4 = _tmp3 * (_tmp0 - _tmp1 + _tmp2);
const Scalar _tmp5 = 2 * state(3, 0);
const Scalar _tmp6 = _tmp5 * state(0, 0);
const Scalar _tmp7 = 2 * state(2, 0);
const Scalar _tmp8 = _tmp7 * state(1, 0);
const Scalar _tmp9 = _tmp3 * (-_tmp6 + _tmp8);
const Scalar _tmp10 = 2 * state(4, 0);
const Scalar _tmp11 = _tmp10 * state(0, 0);
const Scalar _tmp12 = 2 * state(5, 0);
const Scalar _tmp13 = _tmp12 * state(3, 0);
const Scalar _tmp14 = _tmp7 * state(6, 0);
const Scalar _tmp15 = _tmp3 * (-_tmp11 - _tmp13 + _tmp14);
const Scalar _tmp16 = 2 * state(1, 0);
const Scalar _tmp17 =
_tmp3 * (-_tmp10 * state(3, 0) + _tmp12 * state(0, 0) + _tmp16 * state(6, 0));
const Scalar _tmp18 = _tmp7 * state(4, 0);
const Scalar _tmp19 = _tmp12 * state(1, 0);
const Scalar _tmp20 = 2 * state(0, 0) * state(6, 0);
const Scalar _tmp21 = _tmp3 * (_tmp18 - _tmp19 + _tmp20);
const Scalar _tmp22 = _tmp3 * (_tmp10 * state(1, 0) + _tmp5 * state(6, 0) + _tmp7 * state(5, 0));
const Scalar _tmp23 = _tmp3 * (_tmp16 * state(0, 0) + _tmp7 * state(3, 0));
const Scalar _tmp24 = _tmp3 * (-_tmp0 + _tmp1 + _tmp2);
const Scalar _tmp25 = _tmp3 * (_tmp6 + _tmp8);
const Scalar _tmp26 = _tmp3 * (_tmp16 * state(3, 0) - _tmp7 * state(0, 0));
const Scalar _tmp27 = _tmp3 * (_tmp11 + _tmp13 - _tmp14);
const Scalar _tmp28 = _tmp3 * (-_tmp18 + _tmp19 - _tmp20);
// Output terms (2)
if (innov_var != nullptr) {
matrix::Matrix<Scalar, 2, 1>& _innov_var = (*innov_var);
_innov_var(0, 0) =
R +
_tmp15 * (P(0, 3) * _tmp17 + P(1, 3) * _tmp21 + P(2, 3) * _tmp22 + P(3, 3) * _tmp15 +
P(4, 3) * _tmp9 + P(5, 3) * _tmp4 + P(6, 3) * _tmp23) +
_tmp17 * (P(0, 0) * _tmp17 + P(1, 0) * _tmp21 + P(2, 0) * _tmp22 + P(3, 0) * _tmp15 +
P(4, 0) * _tmp9 + P(5, 0) * _tmp4 + P(6, 0) * _tmp23) +
_tmp21 * (P(0, 1) * _tmp17 + P(1, 1) * _tmp21 + P(2, 1) * _tmp22 + P(3, 1) * _tmp15 +
P(4, 1) * _tmp9 + P(5, 1) * _tmp4 + P(6, 1) * _tmp23) +
_tmp22 * (P(0, 2) * _tmp17 + P(1, 2) * _tmp21 + P(2, 2) * _tmp22 + P(3, 2) * _tmp15 +
P(4, 2) * _tmp9 + P(5, 2) * _tmp4 + P(6, 2) * _tmp23) +
_tmp23 * (P(0, 6) * _tmp17 + P(1, 6) * _tmp21 + P(2, 6) * _tmp22 + P(3, 6) * _tmp15 +
P(4, 6) * _tmp9 + P(5, 6) * _tmp4 + P(6, 6) * _tmp23) +
_tmp4 * (P(0, 5) * _tmp17 + P(1, 5) * _tmp21 + P(2, 5) * _tmp22 + P(3, 5) * _tmp15 +
P(4, 5) * _tmp9 + P(5, 5) * _tmp4 + P(6, 5) * _tmp23) +
_tmp9 * (P(0, 4) * _tmp17 + P(1, 4) * _tmp21 + P(2, 4) * _tmp22 + P(3, 4) * _tmp15 +
P(4, 4) * _tmp9 + P(5, 4) * _tmp4 + P(6, 4) * _tmp23);
_innov_var(1, 0) =
R -
_tmp17 * (-P(0, 3) * _tmp27 - P(1, 3) * _tmp22 - P(2, 3) * _tmp28 - P(3, 3) * _tmp17 -
P(4, 3) * _tmp24 - P(5, 3) * _tmp25 - P(6, 3) * _tmp26) -
_tmp22 * (-P(0, 1) * _tmp27 - P(1, 1) * _tmp22 - P(2, 1) * _tmp28 - P(3, 1) * _tmp17 -
P(4, 1) * _tmp24 - P(5, 1) * _tmp25 - P(6, 1) * _tmp26) -
_tmp24 * (-P(0, 4) * _tmp27 - P(1, 4) * _tmp22 - P(2, 4) * _tmp28 - P(3, 4) * _tmp17 -
P(4, 4) * _tmp24 - P(5, 4) * _tmp25 - P(6, 4) * _tmp26) -
_tmp25 * (-P(0, 5) * _tmp27 - P(1, 5) * _tmp22 - P(2, 5) * _tmp28 - P(3, 5) * _tmp17 -
P(4, 5) * _tmp24 - P(5, 5) * _tmp25 - P(6, 5) * _tmp26) -
_tmp26 * (-P(0, 6) * _tmp27 - P(1, 6) * _tmp22 - P(2, 6) * _tmp28 - P(3, 6) * _tmp17 -
P(4, 6) * _tmp24 - P(5, 6) * _tmp25 - P(6, 6) * _tmp26) -
_tmp27 * (-P(0, 0) * _tmp27 - P(1, 0) * _tmp22 - P(2, 0) * _tmp28 - P(3, 0) * _tmp17 -
P(4, 0) * _tmp24 - P(5, 0) * _tmp25 - P(6, 0) * _tmp26) -
_tmp28 * (-P(0, 2) * _tmp27 - P(1, 2) * _tmp22 - P(2, 2) * _tmp28 - P(3, 2) * _tmp17 -
P(4, 2) * _tmp24 - P(5, 2) * _tmp25 - P(6, 2) * _tmp26);
}
if (H != nullptr) {
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
_H.setZero();
_H(0, 0) = _tmp17;
_H(1, 0) = _tmp21;
_H(2, 0) = _tmp22;
_H(3, 0) = _tmp15;
_H(4, 0) = _tmp9;
_H(5, 0) = _tmp4;
_H(6, 0) = _tmp23;
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h
+95
View File
@@ -0,0 +1,95 @@
// -----------------------------------------------------------------------------
// 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_flow_y_innov_var_and_h
*
* Args:
* state: Matrix24_1
* P: Matrix24_24
* distance: Scalar
* R: Scalar
* epsilon: Scalar
*
* Outputs:
* innov_var: Scalar
* H: Matrix24_1
*/
template <typename Scalar>
void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar distance,
const Scalar R, const Scalar epsilon,
Scalar* const innov_var = nullptr,
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
// Total ops: 171
// Input arrays
// Intermediate terms (13)
const Scalar _tmp0 =
Scalar(1.0) /
(distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
const Scalar _tmp1 =
_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 _tmp2 = 2 * state(0, 0);
const Scalar _tmp3 = 2 * state(1, 0);
const Scalar _tmp4 = _tmp0 * (_tmp2 * state(3, 0) + _tmp3 * state(2, 0));
const Scalar _tmp5 = _tmp0 * (-_tmp2 * state(2, 0) + _tmp3 * state(3, 0));
const Scalar _tmp6 = 2 * state(4, 0);
const Scalar _tmp7 = 2 * state(6, 0);
const Scalar _tmp8 = _tmp0 * (_tmp2 * state(5, 0) - _tmp6 * state(3, 0) + _tmp7 * state(1, 0));
const Scalar _tmp9 = 2 * state(5, 0);
const Scalar _tmp10 = _tmp0 * (_tmp2 * state(4, 0) - _tmp7 * state(2, 0) + _tmp9 * state(3, 0));
const Scalar _tmp11 = _tmp0 * (_tmp3 * state(4, 0) + _tmp7 * state(3, 0) + _tmp9 * state(2, 0));
const Scalar _tmp12 = _tmp0 * (_tmp3 * state(5, 0) - _tmp6 * state(2, 0) - _tmp7 * state(0, 0));
// Output terms (2)
if (innov_var != nullptr) {
Scalar& _innov_var = (*innov_var);
_innov_var = R -
_tmp1 * (-P(0, 4) * _tmp10 - P(1, 4) * _tmp11 - P(2, 4) * _tmp12 -
P(3, 4) * _tmp8 - P(4, 4) * _tmp1 - P(5, 4) * _tmp4 - P(6, 4) * _tmp5) -
_tmp10 * (-P(0, 0) * _tmp10 - P(1, 0) * _tmp11 - P(2, 0) * _tmp12 -
P(3, 0) * _tmp8 - P(4, 0) * _tmp1 - P(5, 0) * _tmp4 - P(6, 0) * _tmp5) -
_tmp11 * (-P(0, 1) * _tmp10 - P(1, 1) * _tmp11 - P(2, 1) * _tmp12 -
P(3, 1) * _tmp8 - P(4, 1) * _tmp1 - P(5, 1) * _tmp4 - P(6, 1) * _tmp5) -
_tmp12 * (-P(0, 2) * _tmp10 - P(1, 2) * _tmp11 - P(2, 2) * _tmp12 -
P(3, 2) * _tmp8 - P(4, 2) * _tmp1 - P(5, 2) * _tmp4 - P(6, 2) * _tmp5) -
_tmp4 * (-P(0, 5) * _tmp10 - P(1, 5) * _tmp11 - P(2, 5) * _tmp12 -
P(3, 5) * _tmp8 - P(4, 5) * _tmp1 - P(5, 5) * _tmp4 - P(6, 5) * _tmp5) -
_tmp5 * (-P(0, 6) * _tmp10 - P(1, 6) * _tmp11 - P(2, 6) * _tmp12 -
P(3, 6) * _tmp8 - P(4, 6) * _tmp1 - P(5, 6) * _tmp4 - P(6, 6) * _tmp5) -
_tmp8 * (-P(0, 3) * _tmp10 - P(1, 3) * _tmp11 - P(2, 3) * _tmp12 -
P(3, 3) * _tmp8 - P(4, 3) * _tmp1 - P(5, 3) * _tmp4 - P(6, 3) * _tmp5);
}
if (H != nullptr) {
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
_H.setZero();
_H(0, 0) = -_tmp10;
_H(1, 0) = -_tmp11;
_H(2, 0) = -_tmp12;
_H(3, 0) = -_tmp8;
_H(4, 0) = -_tmp1;
_H(5, 0) = -_tmp4;
_H(6, 0) = -_tmp5;
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym