forked from Archive/PX4-Autopilot
wind_est: auto-generate beta fusion using Symforce
This commit is contained in:
bresch
Mathieu Bresciani
parent
487b84e90b
commit
a7124d3738
|
|
@ -187,56 +187,12 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
|
|||
|
||||
_time_last_beta_fuse = time_now;
|
||||
|
||||
const float v_n = velI(0);
|
||||
const float v_e = velI(1);
|
||||
const float v_d = velI(2);
|
||||
|
||||
// compute sideslip observation vector
|
||||
float HB0 = 2.0f * q_att(0);
|
||||
float HB1 = HB0 * q_att(3);
|
||||
float HB2 = 2.0f * q_att(1);
|
||||
float HB3 = HB2 * q_att(2);
|
||||
float HB4 = v_e - _state(INDEX_W_E);
|
||||
float HB5 = HB1 + HB3;
|
||||
float HB6 = v_n - _state(INDEX_W_N);
|
||||
float HB7 = q_att(0) * q_att(0);
|
||||
float HB8 = q_att(3) * q_att(3);
|
||||
float HB9 = HB7 - HB8;
|
||||
float HB10 = q_att(1) * q_att(1);
|
||||
float HB11 = q_att(2) * q_att(2);
|
||||
float HB12 = HB10 - HB11;
|
||||
float HB13 = HB12 + HB9;
|
||||
float HB14 = HB13 * HB6 + HB4 * HB5 + v_d * (-HB0 * q_att(2) + HB2 * q_att(3));
|
||||
float HB15 = 1.0f / HB14;
|
||||
float HB16 = (HB4 * (-HB10 + HB11 + HB9) + HB6 * (-HB1 + HB3) + v_d * (HB0 * q_att(1) + 2.0f * q_att(2) * q_att(3))) /
|
||||
(HB14 * HB14);
|
||||
|
||||
matrix::Matrix<float, 1, 3> H_beta;
|
||||
H_beta(0, 0) = HB13 * HB16 + HB15 * (HB1 - HB3);
|
||||
H_beta(0, 1) = HB15 * (HB12 - HB7 + HB8) + HB16 * HB5;
|
||||
H_beta(0, 2) = 0;
|
||||
matrix::Matrix<float, 3, 1> K;
|
||||
|
||||
// compute innovation covariance S
|
||||
const matrix::Matrix<float, 1, 1> S = H_beta * _P * H_beta.transpose() + _beta_var;
|
||||
|
||||
// compute Kalman gain
|
||||
matrix::Matrix<float, 3, 1> K = _P * H_beta.transpose();
|
||||
K /= S(0, 0);
|
||||
|
||||
// compute predicted side slip angle
|
||||
matrix::Vector3f rel_wind(velI(0) - _state(INDEX_W_N), velI(1) - _state(INDEX_W_E), velI(2));
|
||||
matrix::Dcmf R_body_to_earth(q_att);
|
||||
rel_wind = R_body_to_earth.transpose() * rel_wind;
|
||||
|
||||
if (fabsf(rel_wind(0)) < 0.1f) {
|
||||
return;
|
||||
}
|
||||
|
||||
// use small angle approximation, sin(x) = x for small x
|
||||
const float beta_pred = rel_wind(1) / rel_wind(0);
|
||||
|
||||
_beta_innov = 0.0f - beta_pred;
|
||||
_beta_innov_var = S(0, 0);
|
||||
sym::FuseBeta(velI, _state, _P, q_att, _beta_var, FLT_EPSILON,
|
||||
&H_beta, &K, &_beta_innov_var, &_beta_innov);
|
||||
|
||||
bool reinit_filter = false;
|
||||
bool meas_is_rejected = false;
|
||||
|
|
|
|||
|
|
@ -44,6 +44,7 @@
|
|||
#include <matrix/math.hpp>
|
||||
|
||||
#include "python/generated/fuse_airspeed.h"
|
||||
#include "python/generated/fuse_beta.h"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
|
|
|
|||
|
|
@ -24,6 +24,56 @@ def fuse_airspeed(
|
|||
|
||||
return (geo.V3(H), K, innov_var, innov)
|
||||
|
||||
# q: quaternion describing rotation from frame 1 to frame 2
|
||||
# returns a rotation matrix derived form q which describes the same
|
||||
# rotation
|
||||
def quat2Rot(q):
|
||||
q0 = q[0]
|
||||
q1 = q[1]
|
||||
q2 = q[2]
|
||||
q3 = q[3]
|
||||
|
||||
Rot = geo.M33([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
|
||||
[2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)],
|
||||
[2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]])
|
||||
|
||||
return Rot
|
||||
|
||||
def sign_no_zero(x) -> T.Scalar:
|
||||
"""
|
||||
Returns -1 if x is negative, 1 if x is positive, and 1 if x is zero
|
||||
"""
|
||||
return 2 * sm.Min(sm.sign(x), 0) + 1
|
||||
|
||||
def add_epsilon_sign(expr, var, eps):
|
||||
return expr.subs(var, var + eps * sign_no_zero(var))
|
||||
|
||||
def fuse_beta(
|
||||
v_local: geo.V3,
|
||||
state: geo.V3,
|
||||
P: geo.M33,
|
||||
q_att: geo.V4,
|
||||
R: T.Scalar,
|
||||
epsilon: T.Scalar
|
||||
) -> (geo.V3, geo.V3, T.Scalar, T.Scalar):
|
||||
|
||||
vel_rel = geo.V3(v_local[0] - state[0], v_local[1] - state[1], v_local[2])
|
||||
relative_wind_body = quat2Rot(q_att).T * vel_rel
|
||||
|
||||
# small angle approximation of side slip model
|
||||
# protect division by zero using epsilon
|
||||
beta_pred = add_epsilon_sign(relative_wind_body[1] / relative_wind_body[0], relative_wind_body[0], epsilon)
|
||||
|
||||
innov = 0.0 - beta_pred
|
||||
|
||||
H = geo.V1(beta_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)
|
||||
|
||||
|
||||
def generatePx4Function(function_name, output_names):
|
||||
from symforce.codegen import Codegen, CppConfig
|
||||
codegen = Codegen.function(
|
||||
|
|
@ -44,6 +94,7 @@ def generatePx4Function(function_name, output_names):
|
|||
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::min", "math::min")
|
||||
line = line.replace("Eigen", "matrix")
|
||||
line = line.replace("matrix/Dense", "matrix/math.hpp")
|
||||
print(line, end='')
|
||||
|
|
@ -61,3 +112,4 @@ def generatePythonFunction(function_name, output_names):
|
|||
|
||||
generatePx4Function(fuse_airspeed, output_names=["H", "K", "innov_var", "innov"])
|
||||
generatePythonFunction(fuse_airspeed, output_names=["H", "K", "innov_var", "innov"])
|
||||
generatePx4Function(fuse_beta, output_names=["H", "K", "innov_var", "innov"])
|
||||
|
|
|
|||
|
|
@ -0,0 +1,102 @@
|
|||
// -----------------------------------------------------------------------------
|
||||
// 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_beta
|
||||
*
|
||||
* Args:
|
||||
* v_local: Matrix31
|
||||
* state: Matrix31
|
||||
* P: Matrix33
|
||||
* q_att: Matrix41
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* H: Matrix13
|
||||
* K: Matrix31
|
||||
* innov_var: Scalar
|
||||
* innov: Scalar
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void FuseBeta(const matrix::Matrix<Scalar, 3, 1>& v_local, const matrix::Matrix<Scalar, 3, 1>& state,
|
||||
const matrix::Matrix<Scalar, 3, 3>& P, const matrix::Matrix<Scalar, 4, 1>& q_att,
|
||||
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: 75
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (23)
|
||||
const Scalar _tmp0 = 2 * v_local(2, 0);
|
||||
const Scalar _tmp1 = std::pow(q_att(1, 0), Scalar(2));
|
||||
const Scalar _tmp2 = std::pow(q_att(2, 0), Scalar(2));
|
||||
const Scalar _tmp3 = std::pow(q_att(0, 0), Scalar(2)) - std::pow(q_att(3, 0), Scalar(2));
|
||||
const Scalar _tmp4 = _tmp1 - _tmp2 + _tmp3;
|
||||
const Scalar _tmp5 = -state(0, 0) + v_local(0, 0);
|
||||
const Scalar _tmp6 = -state(1, 0) + v_local(1, 0);
|
||||
const Scalar _tmp7 = q_att(0, 0) * q_att(3, 0);
|
||||
const Scalar _tmp8 = q_att(1, 0) * q_att(2, 0);
|
||||
const Scalar _tmp9 = 2 * _tmp7 + 2 * _tmp8;
|
||||
const Scalar _tmp10 = _tmp0 * (-q_att(0, 0) * q_att(2, 0) + q_att(1, 0) * q_att(3, 0)) +
|
||||
_tmp4 * _tmp5 + _tmp6 * _tmp9;
|
||||
const Scalar _tmp11 =
|
||||
_tmp10 + epsilon * (2 * math::min<Scalar>(0, (((_tmp10) > 0) - ((_tmp10) < 0))) + 1);
|
||||
const Scalar _tmp12 = Scalar(1.0) / (_tmp11);
|
||||
const Scalar _tmp13 = -2 * _tmp7 + 2 * _tmp8;
|
||||
const Scalar _tmp14 = -_tmp1 + _tmp2 + _tmp3;
|
||||
const Scalar _tmp15 = _tmp0 * (q_att(0, 0) * q_att(1, 0) + q_att(2, 0) * q_att(3, 0)) +
|
||||
_tmp13 * _tmp5 + _tmp14 * _tmp6;
|
||||
const Scalar _tmp16 = _tmp15 / std::pow(_tmp11, Scalar(2));
|
||||
const Scalar _tmp17 = -_tmp12 * _tmp13 + _tmp16 * _tmp4;
|
||||
const Scalar _tmp18 = -_tmp12 * _tmp14 + _tmp16 * _tmp9;
|
||||
const Scalar _tmp19 = P(0, 0) * _tmp17;
|
||||
const Scalar _tmp20 = P(1, 1) * _tmp18;
|
||||
const Scalar _tmp21 =
|
||||
R + _tmp17 * (P(1, 0) * _tmp18 + _tmp19) + _tmp18 * (P(0, 1) * _tmp17 + _tmp20);
|
||||
const Scalar _tmp22 = Scalar(1.0) / (math::max<Scalar>(_tmp21, epsilon));
|
||||
|
||||
// Output terms (4)
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 1, 3>& _H = (*H);
|
||||
|
||||
_H(0, 0) = _tmp17;
|
||||
_H(0, 1) = _tmp18;
|
||||
_H(0, 2) = 0;
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _K = (*K);
|
||||
|
||||
_K(0, 0) = _tmp22 * (P(0, 1) * _tmp18 + _tmp19);
|
||||
_K(1, 0) = _tmp22 * (P(1, 0) * _tmp17 + _tmp20);
|
||||
_K(2, 0) = _tmp22 * (P(2, 0) * _tmp17 + P(2, 1) * _tmp18);
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = _tmp21;
|
||||
}
|
||||
|
||||
if (innov != nullptr) {
|
||||
Scalar& _innov = (*innov);
|
||||
|
||||
_innov = -_tmp12 * _tmp15;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
Loading…
Reference in New Issue