Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 20:03:54 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

ekf2: do not pre-compute airspeed Kalman gain

Browse Source 
The generated code is not much faster than the simple matrix-vector
multiplication
This commit is contained in:
bresch
2024-12-16 15:23:06 +01:00
committed by Daniel Agar
parent e7145e7b44
commit 6969e5b6b4
4 changed files with 62 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp
+3 -5
View File
@@ -44,7 +44,7 @@
#include "ekf.h"
#include <ekf_derivation/generated/compute_airspeed_h_and_k.h>
#include <ekf_derivation/generated/compute_airspeed_h.h>
#include <ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h>
#include <ekf_derivation/generated/compute_wind_init_and_cov_from_airspeed.h>
@@ -204,10 +204,8 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour
_fault_status.flags.bad_airspeed = false;
VectorState H; // Observation jacobian
VectorState K; // Kalman gain vector
sym::ComputeAirspeedHAndK(_state.vector(), P, innov_var, FLT_EPSILON, &H, &K);
const VectorState H = sym::ComputeAirspeedH(_state.vector(), FLT_EPSILON);
VectorState K = P * H / aid_src.innovation_variance;
if (update_wind_only) {
const Vector2f K_wind = K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0);
src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
+3 -7
View File
@@ -268,10 +268,8 @@ def compute_airspeed_innov_and_innov_var(
return (innov, innov_var)
def compute_airspeed_h_and_k(
def compute_airspeed_h(
state: VState,
P: MTangent,
innov_var: sf.Scalar,
epsilon: sf.Scalar
) -> (VTangent, VTangent):
@@ -281,9 +279,7 @@ def compute_airspeed_h_and_k(
airspeed_pred = vel_rel.norm(epsilon=epsilon)
H = jacobian_chain_rule(airspeed_pred, state)
K = P * H.T / sf.Max(innov_var, epsilon)
return (H.T, K)
return H.T
def compute_wind_init_and_cov_from_airspeed(
v_local: sf.V3,
@@ -739,7 +735,7 @@ if not args.disable_mag:
generate_px4_function(compute_mag_z_innov_var_and_h, output_names=["innov_var", "H"])
if not args.disable_wind:
generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"])
generate_px4_function(compute_airspeed_h, output_names=None)
generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
generate_px4_function(compute_drag_x_innov_var_and_h, output_names=["innov_var", "Hx"])
generate_px4_function(compute_drag_y_innov_var_and_h, output_names=["innov_var", "Hy"])
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h.h
+56
View File
@@ -0,0 +1,56 @@
// -----------------------------------------------------------------------------
// 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_airspeed_h
*
* Args:
* state: Matrix25_1
* epsilon: Scalar
*
* Outputs:
* res: Matrix24_1
*/
template <typename Scalar>
matrix::Matrix<Scalar, 24, 1> ComputeAirspeedH(const matrix::Matrix<Scalar, 25, 1>& state,
const Scalar epsilon) {
// Total ops: 14
// Input arrays
// Intermediate terms (5)
const Scalar _tmp0 = -state(23, 0) + state(5, 0);
const Scalar _tmp1 = -state(22, 0) + state(4, 0);
const Scalar _tmp2 = std::pow(Scalar(std::pow(_tmp0, Scalar(2)) + std::pow(_tmp1, Scalar(2)) +
epsilon + std::pow(state(6, 0), Scalar(2))),
Scalar(Scalar(-1) / Scalar(2)));
const Scalar _tmp3 = _tmp1 * _tmp2;
const Scalar _tmp4 = _tmp0 * _tmp2;
// Output terms (1)
matrix::Matrix<Scalar, 24, 1> _res;
_res.setZero();
_res(3, 0) = _tmp3;
_res(4, 0) = _tmp4;
_res(5, 0) = _tmp2 * state(6, 0);
_res(21, 0) = -_tmp3;
_res(22, 0) = -_tmp4;
return _res;
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym
src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h
-116
View File
@@ -1,116 +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_airspeed_h_and_k
*
* Args:
* state: Matrix25_1
* P: Matrix24_24
* innov_var: Scalar
* epsilon: Scalar
*
* Outputs:
* H: Matrix24_1
* K: Matrix24_1
*/
template <typename Scalar>
void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
// Total ops: 256
// Input arrays
// Intermediate terms (7)
const Scalar _tmp0 = -state(23, 0) + state(5, 0);
const Scalar _tmp1 = -state(22, 0) + state(4, 0);
const Scalar _tmp2 = std::pow(Scalar(std::pow(_tmp0, Scalar(2)) + std::pow(_tmp1, Scalar(2)) +
epsilon + std::pow(state(6, 0), Scalar(2))),
Scalar(Scalar(-1) / Scalar(2)));
const Scalar _tmp3 = _tmp1 * _tmp2;
const Scalar _tmp4 = _tmp0 * _tmp2;
const Scalar _tmp5 = _tmp2 * state(6, 0);
const Scalar _tmp6 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
// Output terms (2)
if (H != nullptr) {
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
_h.setZero();
_h(3, 0) = _tmp3;
_h(4, 0) = _tmp4;
_h(5, 0) = _tmp5;
_h(21, 0) = -_tmp3;
_h(22, 0) = -_tmp4;
}
if (K != nullptr) {
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
_k(0, 0) = _tmp6 * (-P(0, 21) * _tmp3 - P(0, 22) * _tmp4 + P(0, 3) * _tmp3 + P(0, 4) * _tmp4 +
P(0, 5) * _tmp5);
_k(1, 0) = _tmp6 * (-P(1, 21) * _tmp3 - P(1, 22) * _tmp4 + P(1, 3) * _tmp3 + P(1, 4) * _tmp4 +
P(1, 5) * _tmp5);
_k(2, 0) = _tmp6 * (-P(2, 21) * _tmp3 - P(2, 22) * _tmp4 + P(2, 3) * _tmp3 + P(2, 4) * _tmp4 +
P(2, 5) * _tmp5);
_k(3, 0) = _tmp6 * (-P(3, 21) * _tmp3 - P(3, 22) * _tmp4 + P(3, 3) * _tmp3 + P(3, 4) * _tmp4 +
P(3, 5) * _tmp5);
_k(4, 0) = _tmp6 * (-P(4, 21) * _tmp3 - P(4, 22) * _tmp4 + P(4, 3) * _tmp3 + P(4, 4) * _tmp4 +
P(4, 5) * _tmp5);
_k(5, 0) = _tmp6 * (-P(5, 21) * _tmp3 - P(5, 22) * _tmp4 + P(5, 3) * _tmp3 + P(5, 4) * _tmp4 +
P(5, 5) * _tmp5);
_k(6, 0) = _tmp6 * (-P(6, 21) * _tmp3 - P(6, 22) * _tmp4 + P(6, 3) * _tmp3 + P(6, 4) * _tmp4 +
P(6, 5) * _tmp5);
_k(7, 0) = _tmp6 * (-P(7, 21) * _tmp3 - P(7, 22) * _tmp4 + P(7, 3) * _tmp3 + P(7, 4) * _tmp4 +
P(7, 5) * _tmp5);
_k(8, 0) = _tmp6 * (-P(8, 21) * _tmp3 - P(8, 22) * _tmp4 + P(8, 3) * _tmp3 + P(8, 4) * _tmp4 +
P(8, 5) * _tmp5);
_k(9, 0) = _tmp6 * (-P(9, 21) * _tmp3 - P(9, 22) * _tmp4 + P(9, 3) * _tmp3 + P(9, 4) * _tmp4 +
P(9, 5) * _tmp5);
_k(10, 0) = _tmp6 * (-P(10, 21) * _tmp3 - P(10, 22) * _tmp4 + P(10, 3) * _tmp3 +
P(10, 4) * _tmp4 + P(10, 5) * _tmp5);
_k(11, 0) = _tmp6 * (-P(11, 21) * _tmp3 - P(11, 22) * _tmp4 + P(11, 3) * _tmp3 +
P(11, 4) * _tmp4 + P(11, 5) * _tmp5);
_k(12, 0) = _tmp6 * (-P(12, 21) * _tmp3 - P(12, 22) * _tmp4 + P(12, 3) * _tmp3 +
P(12, 4) * _tmp4 + P(12, 5) * _tmp5);
_k(13, 0) = _tmp6 * (-P(13, 21) * _tmp3 - P(13, 22) * _tmp4 + P(13, 3) * _tmp3 +
P(13, 4) * _tmp4 + P(13, 5) * _tmp5);
_k(14, 0) = _tmp6 * (-P(14, 21) * _tmp3 - P(14, 22) * _tmp4 + P(14, 3) * _tmp3 +
P(14, 4) * _tmp4 + P(14, 5) * _tmp5);
_k(15, 0) = _tmp6 * (-P(15, 21) * _tmp3 - P(15, 22) * _tmp4 + P(15, 3) * _tmp3 +
P(15, 4) * _tmp4 + P(15, 5) * _tmp5);
_k(16, 0) = _tmp6 * (-P(16, 21) * _tmp3 - P(16, 22) * _tmp4 + P(16, 3) * _tmp3 +
P(16, 4) * _tmp4 + P(16, 5) * _tmp5);
_k(17, 0) = _tmp6 * (-P(17, 21) * _tmp3 - P(17, 22) * _tmp4 + P(17, 3) * _tmp3 +
P(17, 4) * _tmp4 + P(17, 5) * _tmp5);
_k(18, 0) = _tmp6 * (-P(18, 21) * _tmp3 - P(18, 22) * _tmp4 + P(18, 3) * _tmp3 +
P(18, 4) * _tmp4 + P(18, 5) * _tmp5);
_k(19, 0) = _tmp6 * (-P(19, 21) * _tmp3 - P(19, 22) * _tmp4 + P(19, 3) * _tmp3 +
P(19, 4) * _tmp4 + P(19, 5) * _tmp5);
_k(20, 0) = _tmp6 * (-P(20, 21) * _tmp3 - P(20, 22) * _tmp4 + P(20, 3) * _tmp3 +
P(20, 4) * _tmp4 + P(20, 5) * _tmp5);
_k(21, 0) = _tmp6 * (-P(21, 21) * _tmp3 - P(21, 22) * _tmp4 + P(21, 3) * _tmp3 +
P(21, 4) * _tmp4 + P(21, 5) * _tmp5);
_k(22, 0) = _tmp6 * (-P(22, 21) * _tmp3 - P(22, 22) * _tmp4 + P(22, 3) * _tmp3 +
P(22, 4) * _tmp4 + P(22, 5) * _tmp5);
_k(23, 0) = _tmp6 * (-P(23, 21) * _tmp3 - P(23, 22) * _tmp4 + P(23, 3) * _tmp3 +
P(23, 4) * _tmp4 + P(23, 5) * _tmp5);
}
} // NOLINT(readability/fn_size)
// NOLINTNEXTLINE(readability/fn_size)
} // namespace sym