forked from Archive/PX4-Autopilot
WindEstimator: use SymForce auto-generated function for airspeed fusion
This commit is contained in:
bresch
Daniel Agar
parent
e6eed43648
commit
26190a7799
|
|
@ -26,3 +26,5 @@ requests
|
|||
setuptools>=39.2.0
|
||||
six>=1.12.0
|
||||
toml>=0.9
|
||||
symforce>=0.5.0
|
||||
sympy>=1.10.1
|
||||
|
|
|
|||
|
|
@ -141,37 +141,11 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
|
|||
|
||||
_time_last_airspeed_fuse = time_now;
|
||||
|
||||
// assign helper variables
|
||||
const float v_n = velI(0);
|
||||
const float v_e = velI(1);
|
||||
const float v_d = velI(2);
|
||||
|
||||
// calculate airspeed from ground speed and wind states (without scale)
|
||||
const float airspeed_predicted_raw = sqrtf((v_n - _state(INDEX_W_N)) * (v_n - _state(INDEX_W_N)) +
|
||||
(v_e - _state(INDEX_W_E)) * (v_e - _state(INDEX_W_E)) + v_d * v_d);
|
||||
|
||||
// compute state observation matrix H
|
||||
const float HH0 = airspeed_predicted_raw;
|
||||
const float HH1 = _state(INDEX_TAS_SCALE) / math::max(HH0, 0.1f);
|
||||
|
||||
matrix::Matrix<float, 1, 3> H_tas;
|
||||
H_tas(0, 0) = HH1 * (-v_n + _state(INDEX_W_N));
|
||||
H_tas(0, 1) = HH1 * (-v_e + _state(INDEX_W_E));
|
||||
H_tas(0, 2) = HH0;
|
||||
matrix::Matrix<float, 3, 1> K;
|
||||
|
||||
// compute innovation covariance S
|
||||
const matrix::Matrix<float, 1, 1> S = H_tas * _P * H_tas.transpose() + _tas_var;
|
||||
|
||||
// compute Kalman gain
|
||||
matrix::Matrix<float, 3, 1> K = _P * H_tas.transpose();
|
||||
K /= S(0, 0);
|
||||
|
||||
// compute innovation
|
||||
const float airspeed_pred = _state(INDEX_TAS_SCALE) * airspeed_predicted_raw;
|
||||
_tas_innov = true_airspeed - airspeed_pred;
|
||||
|
||||
// innovation variance
|
||||
_tas_innov_var = S(0, 0);
|
||||
sym::FuseAirspeed(velI, _state, _P, true_airspeed, _tas_var, FLT_EPSILON,
|
||||
&H_tas, &K, &_tas_innov_var, &_tas_innov);
|
||||
|
||||
bool reinit_filter = false;
|
||||
bool meas_is_rejected = false;
|
||||
|
|
|
|||
|
|
@ -41,6 +41,8 @@
|
|||
#include <mathlib/mathlib.h>
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
#include "python/generated/fuse_airspeed.h"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class WindEstimator
|
||||
|
|
|
|||
|
|
@ -0,0 +1,50 @@
|
|||
import os
|
||||
from symforce import symbolic as sm
|
||||
from symforce import geo
|
||||
from symforce import typing as T
|
||||
|
||||
def fuse_airspeed(
|
||||
v_local: geo.V3,
|
||||
state: geo.V3,
|
||||
P: geo.M33,
|
||||
airspeed: T.Scalar,
|
||||
R: T.Scalar,
|
||||
epsilon: T.Scalar
|
||||
) -> geo.V3:
|
||||
|
||||
vel_rel = geo.V3(v_local[0] - state[0], v_local[1] - state[1], v_local[2])
|
||||
airspeed_pred = vel_rel.norm(epsilon=epsilon) * state[2]
|
||||
|
||||
innov = airspeed - airspeed_pred
|
||||
|
||||
H = geo.V1(airspeed_pred).jacobian(state)
|
||||
innov_var = (H * P * H.transpose() + R)[0,0]
|
||||
|
||||
K = P * H.transpose() / sm.Max(innov_var, epsilon)
|
||||
|
||||
return (geo.V3(H), K, innov_var, innov)
|
||||
|
||||
from symforce.codegen import Codegen, CppConfig
|
||||
|
||||
codegen = Codegen.function(
|
||||
fuse_airspeed,
|
||||
output_names=["H", "K", "innov_var", "innov"],
|
||||
config=CppConfig())
|
||||
metadata = codegen.generate_function(
|
||||
output_dir="generated",
|
||||
skip_directory_nesting=True)
|
||||
|
||||
print("Files generated in {}:\n".format(metadata.output_dir))
|
||||
for f in metadata.generated_files:
|
||||
print(" |- {}".format(os.path.relpath(f, metadata.output_dir)))
|
||||
|
||||
# Replace cstdlib and Eigen functions by PX4 equivalents
|
||||
import fileinput
|
||||
|
||||
with fileinput.FileInput(os.path.abspath(metadata.generated_files[0]), inplace=True, backup='.bak') as file:
|
||||
for line in file:
|
||||
line = line.replace("std::max", "math::max")
|
||||
line = line.replace("std", "matrix")
|
||||
line = line.replace("Eigen", "matrix")
|
||||
line = line.replace("matrix/Dense", "matrix/math.hpp")
|
||||
print(line, end='')
|
||||
|
|
@ -0,0 +1,90 @@
|
|||
// -----------------------------------------------------------------------------
|
||||
// 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: fuse_airspeed
|
||||
*
|
||||
* Args:
|
||||
* v_local: Matrix31
|
||||
* state: Matrix31
|
||||
* P: Matrix33
|
||||
* airspeed: Scalar
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* H: Matrix13
|
||||
* K: Matrix31
|
||||
* innov_var: Scalar
|
||||
* innov: Scalar
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void FuseAirspeed(const matrix::Matrix<Scalar, 3, 1>& v_local,
|
||||
const matrix::Matrix<Scalar, 3, 1>& state, const matrix::Matrix<Scalar, 3, 3>& P,
|
||||
const Scalar airspeed, const Scalar R, const Scalar epsilon,
|
||||
matrix::Matrix<Scalar, 1, 3>* const H = nullptr,
|
||||
matrix::Matrix<Scalar, 3, 1>* const K = nullptr, Scalar* const innov_var = nullptr,
|
||||
Scalar* const innov = nullptr) {
|
||||
// Total ops: 56
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (11)
|
||||
const Scalar _tmp0 = -state(0, 0) + v_local(0, 0);
|
||||
const Scalar _tmp1 = -state(1, 0) + v_local(1, 0);
|
||||
const Scalar _tmp2 = matrix::sqrt(Scalar(matrix::pow(_tmp0, Scalar(2)) + matrix::pow(_tmp1, Scalar(2)) +
|
||||
epsilon + matrix::pow(v_local(2, 0), Scalar(2))));
|
||||
const Scalar _tmp3 = state(2, 0) / _tmp2;
|
||||
const Scalar _tmp4 = _tmp0 * _tmp3;
|
||||
const Scalar _tmp5 = _tmp1 * _tmp3;
|
||||
const Scalar _tmp6 = -P(0, 0) * _tmp4;
|
||||
const Scalar _tmp7 = -P(1, 1) * _tmp5;
|
||||
const Scalar _tmp8 = P(2, 2) * _tmp2;
|
||||
const Scalar _tmp9 = R + _tmp2 * (-P(0, 2) * _tmp4 - P(1, 2) * _tmp5 + _tmp8) -
|
||||
_tmp4 * (-P(1, 0) * _tmp5 + P(2, 0) * _tmp2 + _tmp6) -
|
||||
_tmp5 * (-P(0, 1) * _tmp4 + P(2, 1) * _tmp2 + _tmp7);
|
||||
const Scalar _tmp10 = Scalar(1.0) / (math::max<Scalar>(_tmp9, epsilon));
|
||||
|
||||
// Output terms (4)
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 1, 3>& _H = (*H);
|
||||
|
||||
_H(0, 0) = -_tmp4;
|
||||
_H(0, 1) = -_tmp5;
|
||||
_H(0, 2) = _tmp2;
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _K = (*K);
|
||||
|
||||
_K(0, 0) = _tmp10 * (-P(0, 1) * _tmp5 + P(0, 2) * _tmp2 + _tmp6);
|
||||
_K(1, 0) = _tmp10 * (-P(1, 0) * _tmp4 + P(1, 2) * _tmp2 + _tmp7);
|
||||
_K(2, 0) = _tmp10 * (-P(2, 0) * _tmp4 - P(2, 1) * _tmp5 + _tmp8);
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = _tmp9;
|
||||
}
|
||||
|
||||
if (innov != nullptr) {
|
||||
Scalar& _innov = (*innov);
|
||||
|
||||
_innov = -_tmp2 * state(2, 0) + airspeed;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
Loading…
Reference in New Issue