mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
ekf2: migrate drag fusion derivation to SymForce
This commit is contained in:
bresch
Daniel Agar
parent
04f76c932e
commit
420f5ef2b7
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
|
||||
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@ -12,7 +12,7 @@
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name ECL nor the names of its contributors may be
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
@ -34,27 +34,22 @@
|
||||
/**
|
||||
* @file drag_fusion.cpp
|
||||
* Body frame drag fusion methods used for multi-rotor wind estimation.
|
||||
* equations generated using EKF/python/ekf_derivation/main.py
|
||||
*
|
||||
* @author Paul Riseborough <p_riseborough@live.com.au>
|
||||
*
|
||||
*/
|
||||
|
||||
#include "ekf.h"
|
||||
#include "python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h"
|
||||
#include "python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h"
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
void Ekf::fuseDrag(const dragSample &drag_sample)
|
||||
{
|
||||
SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion; // Observation Jacobians
|
||||
Vector24f Kfusion; // Kalman gain vector
|
||||
|
||||
const float R_ACC = fmaxf(_params.drag_noise, 0.5f); // observation noise variance in specific force drag (m/sec**2)**2
|
||||
const float rho = fmaxf(_air_density, 0.1f); // air density (kg/m**3)
|
||||
|
||||
// correct rotor momentum drag for increase in required rotor mass flow with altitude
|
||||
// obtained from momentum disc theory
|
||||
const float mcoef_corrrected = _params.mcoef * sqrtf(rho / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
|
||||
const float mcoef_corrrected = fmaxf(_params.mcoef * sqrtf(rho / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C), 0.f);
|
||||
|
||||
// drag model parameters
|
||||
const bool using_bcoef_x = _params.bcoef_x > 1.0f;
|
||||
@ -65,26 +60,16 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
|
||||
return;
|
||||
}
|
||||
|
||||
// get latest estimated orientation
|
||||
const float q0 = _state.quat_nominal(0);
|
||||
const float q1 = _state.quat_nominal(1);
|
||||
const float q2 = _state.quat_nominal(2);
|
||||
const float q3 = _state.quat_nominal(3);
|
||||
|
||||
// get latest velocity in earth frame
|
||||
const float vn = _state.vel(0);
|
||||
const float ve = _state.vel(1);
|
||||
const float vd = _state.vel(2);
|
||||
|
||||
// get latest wind velocity in earth frame
|
||||
const float vwn = _state.wind_vel(0);
|
||||
const float vwe = _state.wind_vel(1);
|
||||
|
||||
// predicted specific forces
|
||||
// calculate relative wind velocity in earth frame and rotate into body frame
|
||||
const Vector3f rel_wind_earth(vn - vwn, ve - vwe, vd);
|
||||
const Vector3f rel_wind_earth(_state.vel(0) - _state.wind_vel(0),
|
||||
_state.vel(1) - _state.wind_vel(1),
|
||||
_state.vel(2));
|
||||
const Vector3f rel_wind_body = _state.quat_nominal.rotateVectorInverse(rel_wind_earth);
|
||||
const float rel_wind_speed = rel_wind_body.norm();
|
||||
const Vector24f state_vector_prev = getStateAtFusionHorizonAsVector();
|
||||
|
||||
Vector24f Kfusion;
|
||||
|
||||
// perform sequential fusion of XY specific forces
|
||||
for (uint8_t axis_index = 0; axis_index < 2; axis_index++) {
|
||||
@ -94,236 +79,30 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
|
||||
// Drag is modelled as an arbitrary combination of bluff body drag that proportional to
|
||||
// equivalent airspeed squared, and rotor momentum drag that is proportional to true airspeed
|
||||
// parallel to the rotor disc and mass flow through the rotor disc.
|
||||
float pred_acc = 0.0f; // predicted drag acceleration
|
||||
float bcoef_inv = 0.f;
|
||||
|
||||
if (axis_index == 0) {
|
||||
float Kacc; // Derivative of specific force wrt airspeed
|
||||
|
||||
if (using_mcoef && using_bcoef_x) {
|
||||
// Use a combination of bluff body and propeller momentum drag
|
||||
const float bcoef_inv = 1.0f / _params.bcoef_x;
|
||||
// The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic
|
||||
// mea_acc = 0.5 * rho * bcoef_inv * airspeed**2 + mcoef_corrrected * airspeed
|
||||
// TODO: this should be modified as well and in the derivation: mea_acc = 0.5 * rho * bcoef_inv * airspeed * airspeed_norm + mcoef_corrrected * airspeed
|
||||
const float airspeed = (_params.bcoef_x / rho) * (- mcoef_corrrected + sqrtf(sq(mcoef_corrrected) + 2.0f * rho * bcoef_inv * fabsf(mea_acc)));
|
||||
Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed + mcoef_corrrected);
|
||||
pred_acc = -0.5f * bcoef_inv * rho * rel_wind_body(0) * rel_wind_speed - rel_wind_body(0) * mcoef_corrrected;
|
||||
|
||||
} else if (using_mcoef) {
|
||||
// Use propeller momentum drag only
|
||||
Kacc = fmaxf(1e-1f, mcoef_corrrected);
|
||||
pred_acc = -rel_wind_body(0) * mcoef_corrrected;
|
||||
|
||||
} else if (using_bcoef_x) {
|
||||
// Use bluff body drag only
|
||||
// The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic
|
||||
// mea_acc = (0.5 * rho / _params.bcoef_x) * airspeed**2
|
||||
const float airspeed = sqrtf((2.0f * _params.bcoef_x * fabsf(mea_acc)) / rho);
|
||||
const float bcoef_inv = 1.0f / _params.bcoef_x;
|
||||
Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed);
|
||||
pred_acc = -0.5f * bcoef_inv * rho * rel_wind_body(0) * rel_wind_speed;
|
||||
|
||||
} else {
|
||||
// skip this axis
|
||||
continue;
|
||||
if (using_bcoef_x) {
|
||||
bcoef_inv = 1.0f / _params.bcoef_x;
|
||||
}
|
||||
|
||||
// intermediate variables
|
||||
const float HK0 = vn - vwn;
|
||||
const float HK1 = ve - vwe;
|
||||
const float HK2 = HK0*q0 + HK1*q3 - q2*vd;
|
||||
const float HK3 = 2*Kacc;
|
||||
const float HK4 = HK0*q1 + HK1*q2 + q3*vd;
|
||||
const float HK5 = HK0*q2 - HK1*q1 + q0*vd;
|
||||
const float HK6 = -HK0*q3 + HK1*q0 + q1*vd;
|
||||
const float HK7 = ecl::powf(q0, 2) + ecl::powf(q1, 2) - ecl::powf(q2, 2) - ecl::powf(q3, 2);
|
||||
const float HK8 = HK7*Kacc;
|
||||
const float HK9 = q0*q3 + q1*q2;
|
||||
const float HK10 = HK3*HK9;
|
||||
const float HK11 = q0*q2 - q1*q3;
|
||||
const float HK12 = 2*HK9;
|
||||
const float HK13 = 2*HK11;
|
||||
const float HK14 = 2*HK4;
|
||||
const float HK15 = 2*HK2;
|
||||
const float HK16 = 2*HK5;
|
||||
const float HK17 = 2*HK6;
|
||||
const float HK18 = -HK12*P(0,23) + HK12*P(0,5) - HK13*P(0,6) + HK14*P(0,1) + HK15*P(0,0) - HK16*P(0,2) + HK17*P(0,3) - HK7*P(0,22) + HK7*P(0,4);
|
||||
const float HK19 = HK12*P(5,23);
|
||||
const float HK20 = -HK12*P(23,23) - HK13*P(6,23) + HK14*P(1,23) + HK15*P(0,23) - HK16*P(2,23) + HK17*P(3,23) + HK19 - HK7*P(22,23) + HK7*P(4,23);
|
||||
const float HK21 = ecl::powf(Kacc, 2);
|
||||
const float HK22 = HK12*HK21;
|
||||
const float HK23 = HK12*P(5,5) - HK13*P(5,6) + HK14*P(1,5) + HK15*P(0,5) - HK16*P(2,5) + HK17*P(3,5) - HK19 + HK7*P(4,5) - HK7*P(5,22);
|
||||
const float HK24 = HK12*P(5,6) - HK12*P(6,23) - HK13*P(6,6) + HK14*P(1,6) + HK15*P(0,6) - HK16*P(2,6) + HK17*P(3,6) + HK7*P(4,6) - HK7*P(6,22);
|
||||
const float HK25 = HK7*P(4,22);
|
||||
const float HK26 = -HK12*P(4,23) + HK12*P(4,5) - HK13*P(4,6) + HK14*P(1,4) + HK15*P(0,4) - HK16*P(2,4) + HK17*P(3,4) - HK25 + HK7*P(4,4);
|
||||
const float HK27 = HK21*HK7;
|
||||
const float HK28 = -HK12*P(22,23) + HK12*P(5,22) - HK13*P(6,22) + HK14*P(1,22) + HK15*P(0,22) - HK16*P(2,22) + HK17*P(3,22) + HK25 - HK7*P(22,22);
|
||||
const float HK29 = -HK12*P(1,23) + HK12*P(1,5) - HK13*P(1,6) + HK14*P(1,1) + HK15*P(0,1) - HK16*P(1,2) + HK17*P(1,3) - HK7*P(1,22) + HK7*P(1,4);
|
||||
const float HK30 = -HK12*P(2,23) + HK12*P(2,5) - HK13*P(2,6) + HK14*P(1,2) + HK15*P(0,2) - HK16*P(2,2) + HK17*P(2,3) - HK7*P(2,22) + HK7*P(2,4);
|
||||
const float HK31 = -HK12*P(3,23) + HK12*P(3,5) - HK13*P(3,6) + HK14*P(1,3) + HK15*P(0,3) - HK16*P(2,3) + HK17*P(3,3) - HK7*P(3,22) + HK7*P(3,4);
|
||||
//const float HK32 = Kacc/(-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC);
|
||||
|
||||
// calculate innovation variance and exit if badly conditioned
|
||||
_drag_innov_var(0) = (-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC);
|
||||
if (_drag_innov_var(0) < R_ACC) {
|
||||
return;
|
||||
}
|
||||
|
||||
const float HK32 = Kacc / _drag_innov_var(0);
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = -HK2*HK3;
|
||||
Hfusion.at<1>() = -HK3*HK4;
|
||||
Hfusion.at<2>() = HK3*HK5;
|
||||
Hfusion.at<3>() = -HK3*HK6;
|
||||
Hfusion.at<4>() = -HK8;
|
||||
Hfusion.at<5>() = -HK10;
|
||||
Hfusion.at<6>() = HK11*HK3;
|
||||
Hfusion.at<22>() = HK8;
|
||||
Hfusion.at<23>() = HK10;
|
||||
|
||||
// Kalman gains
|
||||
// Don't allow modification of any states other than wind velocity at this stage of development - we only need a wind estimate.
|
||||
// Kfusion(0) = -HK18*HK32;
|
||||
// Kfusion(1) = -HK29*HK32;
|
||||
// Kfusion(2) = -HK30*HK32;
|
||||
// Kfusion(3) = -HK31*HK32;
|
||||
// Kfusion(4) = -HK26*HK32;
|
||||
// Kfusion(5) = -HK23*HK32;
|
||||
// Kfusion(6) = -HK24*HK32;
|
||||
// Kfusion(7) = -HK32*(HK12*P(5,7) - HK12*P(7,23) - HK13*P(6,7) + HK14*P(1,7) + HK15*P(0,7) - HK16*P(2,7) + HK17*P(3,7) + HK7*P(4,7) - HK7*P(7,22));
|
||||
// Kfusion(8) = -HK32*(HK12*P(5,8) - HK12*P(8,23) - HK13*P(6,8) + HK14*P(1,8) + HK15*P(0,8) - HK16*P(2,8) + HK17*P(3,8) + HK7*P(4,8) - HK7*P(8,22));
|
||||
// Kfusion(9) = -HK32*(HK12*P(5,9) - HK12*P(9,23) - HK13*P(6,9) + HK14*P(1,9) + HK15*P(0,9) - HK16*P(2,9) + HK17*P(3,9) + HK7*P(4,9) - HK7*P(9,22));
|
||||
// Kfusion(10) = -HK32*(-HK12*P(10,23) + HK12*P(5,10) - HK13*P(6,10) + HK14*P(1,10) + HK15*P(0,10) - HK16*P(2,10) + HK17*P(3,10) - HK7*P(10,22) + HK7*P(4,10));
|
||||
// Kfusion(11) = -HK32*(-HK12*P(11,23) + HK12*P(5,11) - HK13*P(6,11) + HK14*P(1,11) + HK15*P(0,11) - HK16*P(2,11) + HK17*P(3,11) - HK7*P(11,22) + HK7*P(4,11));
|
||||
// Kfusion(12) = -HK32*(-HK12*P(12,23) + HK12*P(5,12) - HK13*P(6,12) + HK14*P(1,12) + HK15*P(0,12) - HK16*P(2,12) + HK17*P(3,12) - HK7*P(12,22) + HK7*P(4,12));
|
||||
// Kfusion(13) = -HK32*(-HK12*P(13,23) + HK12*P(5,13) - HK13*P(6,13) + HK14*P(1,13) + HK15*P(0,13) - HK16*P(2,13) + HK17*P(3,13) - HK7*P(13,22) + HK7*P(4,13));
|
||||
// Kfusion(14) = -HK32*(-HK12*P(14,23) + HK12*P(5,14) - HK13*P(6,14) + HK14*P(1,14) + HK15*P(0,14) - HK16*P(2,14) + HK17*P(3,14) - HK7*P(14,22) + HK7*P(4,14));
|
||||
// Kfusion(15) = -HK32*(-HK12*P(15,23) + HK12*P(5,15) - HK13*P(6,15) + HK14*P(1,15) + HK15*P(0,15) - HK16*P(2,15) + HK17*P(3,15) - HK7*P(15,22) + HK7*P(4,15));
|
||||
// Kfusion(16) = -HK32*(-HK12*P(16,23) + HK12*P(5,16) - HK13*P(6,16) + HK14*P(1,16) + HK15*P(0,16) - HK16*P(2,16) + HK17*P(3,16) - HK7*P(16,22) + HK7*P(4,16));
|
||||
// Kfusion(17) = -HK32*(-HK12*P(17,23) + HK12*P(5,17) - HK13*P(6,17) + HK14*P(1,17) + HK15*P(0,17) - HK16*P(2,17) + HK17*P(3,17) - HK7*P(17,22) + HK7*P(4,17));
|
||||
// Kfusion(18) = -HK32*(-HK12*P(18,23) + HK12*P(5,18) - HK13*P(6,18) + HK14*P(1,18) + HK15*P(0,18) - HK16*P(2,18) + HK17*P(3,18) - HK7*P(18,22) + HK7*P(4,18));
|
||||
// Kfusion(19) = -HK32*(-HK12*P(19,23) + HK12*P(5,19) - HK13*P(6,19) + HK14*P(1,19) + HK15*P(0,19) - HK16*P(2,19) + HK17*P(3,19) - HK7*P(19,22) + HK7*P(4,19));
|
||||
// Kfusion(20) = -HK32*(-HK12*P(20,23) + HK12*P(5,20) - HK13*P(6,20) + HK14*P(1,20) + HK15*P(0,20) - HK16*P(2,20) + HK17*P(3,20) - HK7*P(20,22) + HK7*P(4,20));
|
||||
// Kfusion(21) = -HK32*(-HK12*P(21,23) + HK12*P(5,21) - HK13*P(6,21) + HK14*P(1,21) + HK15*P(0,21) - HK16*P(2,21) + HK17*P(3,21) - HK7*P(21,22) + HK7*P(4,21));
|
||||
Kfusion(22) = -HK28*HK32;
|
||||
Kfusion(23) = -HK20*HK32;
|
||||
|
||||
sym::ComputeDragXInnovVarAndK(state_vector_prev, P, rho, bcoef_inv, mcoef_corrrected, R_ACC, FLT_EPSILON, &_drag_innov_var(0), &Kfusion);
|
||||
|
||||
} else if (axis_index == 1) {
|
||||
float Kacc; // Derivative of specific force wrt airspeed
|
||||
|
||||
if (using_mcoef && using_bcoef_y) {
|
||||
// Use a combination of bluff body and propeller momentum drag
|
||||
const float bcoef_inv = 1.0f / _params.bcoef_y;
|
||||
// The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic
|
||||
// mea_acc = 0.5 * rho * bcoef_inv * airspeed**2 + mcoef_corrrected * airspeed
|
||||
const float airspeed = (_params.bcoef_y / rho) * (- mcoef_corrrected + sqrtf(sq(mcoef_corrrected) + 2.0f * rho * bcoef_inv * fabsf(mea_acc)));
|
||||
Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed + mcoef_corrrected);
|
||||
pred_acc = -0.5f * bcoef_inv * rho * rel_wind_body(1) * rel_wind_speed - rel_wind_body(1) * mcoef_corrrected;
|
||||
|
||||
} else if (using_mcoef) {
|
||||
// Use propeller momentum drag only
|
||||
Kacc = fmaxf(1e-1f, mcoef_corrrected);
|
||||
pred_acc = -rel_wind_body(1) * mcoef_corrrected;
|
||||
|
||||
} else if (using_bcoef_y) {
|
||||
// Use bluff body drag only
|
||||
// The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic
|
||||
// mea_acc = (0.5 * rho / _params.bcoef_y) * airspeed**2
|
||||
const float airspeed = sqrtf((2.0f * _params.bcoef_y * fabsf(mea_acc)) / rho);
|
||||
const float bcoef_inv = 1.0f / _params.bcoef_y;
|
||||
Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed);
|
||||
pred_acc = -0.5f * bcoef_inv * rho * rel_wind_body(1) * rel_wind_speed;
|
||||
|
||||
} else {
|
||||
// nothing more to do
|
||||
return;
|
||||
if (using_bcoef_y) {
|
||||
bcoef_inv = 1.0f / _params.bcoef_y;
|
||||
}
|
||||
|
||||
// intermediate variables
|
||||
const float HK0 = ve - vwe;
|
||||
const float HK1 = vn - vwn;
|
||||
const float HK2 = HK0*q0 - HK1*q3 + q1*vd;
|
||||
const float HK3 = 2*Kacc;
|
||||
const float HK4 = -HK0*q1 + HK1*q2 + q0*vd;
|
||||
const float HK5 = HK0*q2 + HK1*q1 + q3*vd;
|
||||
const float HK6 = HK0*q3 + HK1*q0 - q2*vd;
|
||||
const float HK7 = q0*q3 - q1*q2;
|
||||
const float HK8 = HK3*HK7;
|
||||
const float HK9 = ecl::powf(q0, 2) - ecl::powf(q1, 2) + ecl::powf(q2, 2) - ecl::powf(q3, 2);
|
||||
const float HK10 = HK9*Kacc;
|
||||
const float HK11 = q0*q1 + q2*q3;
|
||||
const float HK12 = 2*HK11;
|
||||
const float HK13 = 2*HK7;
|
||||
const float HK14 = 2*HK5;
|
||||
const float HK15 = 2*HK2;
|
||||
const float HK16 = 2*HK4;
|
||||
const float HK17 = 2*HK6;
|
||||
const float HK18 = HK12*P(0,6) + HK13*P(0,22) - HK13*P(0,4) + HK14*P(0,2) + HK15*P(0,0) + HK16*P(0,1) - HK17*P(0,3) - HK9*P(0,23) + HK9*P(0,5);
|
||||
const float HK19 = ecl::powf(Kacc, 2);
|
||||
const float HK20 = HK12*P(6,6) - HK13*P(4,6) + HK13*P(6,22) + HK14*P(2,6) + HK15*P(0,6) + HK16*P(1,6) - HK17*P(3,6) + HK9*P(5,6) - HK9*P(6,23);
|
||||
const float HK21 = HK13*P(4,22);
|
||||
const float HK22 = HK12*P(6,22) + HK13*P(22,22) + HK14*P(2,22) + HK15*P(0,22) + HK16*P(1,22) - HK17*P(3,22) - HK21 - HK9*P(22,23) + HK9*P(5,22);
|
||||
const float HK23 = HK13*HK19;
|
||||
const float HK24 = HK12*P(4,6) - HK13*P(4,4) + HK14*P(2,4) + HK15*P(0,4) + HK16*P(1,4) - HK17*P(3,4) + HK21 - HK9*P(4,23) + HK9*P(4,5);
|
||||
const float HK25 = HK9*P(5,23);
|
||||
const float HK26 = HK12*P(5,6) - HK13*P(4,5) + HK13*P(5,22) + HK14*P(2,5) + HK15*P(0,5) + HK16*P(1,5) - HK17*P(3,5) - HK25 + HK9*P(5,5);
|
||||
const float HK27 = HK19*HK9;
|
||||
const float HK28 = HK12*P(6,23) + HK13*P(22,23) - HK13*P(4,23) + HK14*P(2,23) + HK15*P(0,23) + HK16*P(1,23) - HK17*P(3,23) + HK25 - HK9*P(23,23);
|
||||
const float HK29 = HK12*P(2,6) + HK13*P(2,22) - HK13*P(2,4) + HK14*P(2,2) + HK15*P(0,2) + HK16*P(1,2) - HK17*P(2,3) - HK9*P(2,23) + HK9*P(2,5);
|
||||
const float HK30 = HK12*P(1,6) + HK13*P(1,22) - HK13*P(1,4) + HK14*P(1,2) + HK15*P(0,1) + HK16*P(1,1) - HK17*P(1,3) - HK9*P(1,23) + HK9*P(1,5);
|
||||
const float HK31 = HK12*P(3,6) + HK13*P(3,22) - HK13*P(3,4) + HK14*P(2,3) + HK15*P(0,3) + HK16*P(1,3) - HK17*P(3,3) - HK9*P(3,23) + HK9*P(3,5);
|
||||
// const float HK32 = Kacc/(HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC);
|
||||
|
||||
_drag_innov_var(1) = (HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC);
|
||||
if (_drag_innov_var(1) < R_ACC) {
|
||||
// calculation is badly conditioned
|
||||
return;
|
||||
}
|
||||
|
||||
const float HK32 = Kacc / _drag_innov_var(1);
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion.at<0>() = -HK2*HK3;
|
||||
Hfusion.at<1>() = -HK3*HK4;
|
||||
Hfusion.at<2>() = -HK3*HK5;
|
||||
Hfusion.at<3>() = HK3*HK6;
|
||||
Hfusion.at<4>() = HK8;
|
||||
Hfusion.at<5>() = -HK10;
|
||||
Hfusion.at<6>() = -HK11*HK3;
|
||||
Hfusion.at<22>() = -HK8;
|
||||
Hfusion.at<23>() = HK10;
|
||||
|
||||
// Kalman gains
|
||||
// Don't allow modification of any states other than wind velocity at this stage of development - we only need a wind estimate.
|
||||
// Kfusion(0) = -HK18*HK32;
|
||||
// Kfusion(1) = -HK30*HK32;
|
||||
// Kfusion(2) = -HK29*HK32;
|
||||
// Kfusion(3) = -HK31*HK32;
|
||||
// Kfusion(4) = -HK24*HK32;
|
||||
// Kfusion(5) = -HK26*HK32;
|
||||
// Kfusion(6) = -HK20*HK32;
|
||||
// Kfusion(7) = -HK32*(HK12*P(6,7) - HK13*P(4,7) + HK13*P(7,22) + HK14*P(2,7) + HK15*P(0,7) + HK16*P(1,7) - HK17*P(3,7) + HK9*P(5,7) - HK9*P(7,23));
|
||||
// Kfusion(8) = -HK32*(HK12*P(6,8) - HK13*P(4,8) + HK13*P(8,22) + HK14*P(2,8) + HK15*P(0,8) + HK16*P(1,8) - HK17*P(3,8) + HK9*P(5,8) - HK9*P(8,23));
|
||||
// Kfusion(9) = -HK32*(HK12*P(6,9) - HK13*P(4,9) + HK13*P(9,22) + HK14*P(2,9) + HK15*P(0,9) + HK16*P(1,9) - HK17*P(3,9) + HK9*P(5,9) - HK9*P(9,23));
|
||||
// Kfusion(10) = -HK32*(HK12*P(6,10) + HK13*P(10,22) - HK13*P(4,10) + HK14*P(2,10) + HK15*P(0,10) + HK16*P(1,10) - HK17*P(3,10) - HK9*P(10,23) + HK9*P(5,10));
|
||||
// Kfusion(11) = -HK32*(HK12*P(6,11) + HK13*P(11,22) - HK13*P(4,11) + HK14*P(2,11) + HK15*P(0,11) + HK16*P(1,11) - HK17*P(3,11) - HK9*P(11,23) + HK9*P(5,11));
|
||||
// Kfusion(12) = -HK32*(HK12*P(6,12) + HK13*P(12,22) - HK13*P(4,12) + HK14*P(2,12) + HK15*P(0,12) + HK16*P(1,12) - HK17*P(3,12) - HK9*P(12,23) + HK9*P(5,12));
|
||||
// Kfusion(13) = -HK32*(HK12*P(6,13) + HK13*P(13,22) - HK13*P(4,13) + HK14*P(2,13) + HK15*P(0,13) + HK16*P(1,13) - HK17*P(3,13) - HK9*P(13,23) + HK9*P(5,13));
|
||||
// Kfusion(14) = -HK32*(HK12*P(6,14) + HK13*P(14,22) - HK13*P(4,14) + HK14*P(2,14) + HK15*P(0,14) + HK16*P(1,14) - HK17*P(3,14) - HK9*P(14,23) + HK9*P(5,14));
|
||||
// Kfusion(15) = -HK32*(HK12*P(6,15) + HK13*P(15,22) - HK13*P(4,15) + HK14*P(2,15) + HK15*P(0,15) + HK16*P(1,15) - HK17*P(3,15) - HK9*P(15,23) + HK9*P(5,15));
|
||||
// Kfusion(16) = -HK32*(HK12*P(6,16) + HK13*P(16,22) - HK13*P(4,16) + HK14*P(2,16) + HK15*P(0,16) + HK16*P(1,16) - HK17*P(3,16) - HK9*P(16,23) + HK9*P(5,16));
|
||||
// Kfusion(17) = -HK32*(HK12*P(6,17) + HK13*P(17,22) - HK13*P(4,17) + HK14*P(2,17) + HK15*P(0,17) + HK16*P(1,17) - HK17*P(3,17) - HK9*P(17,23) + HK9*P(5,17));
|
||||
// Kfusion(18) = -HK32*(HK12*P(6,18) + HK13*P(18,22) - HK13*P(4,18) + HK14*P(2,18) + HK15*P(0,18) + HK16*P(1,18) - HK17*P(3,18) - HK9*P(18,23) + HK9*P(5,18));
|
||||
// Kfusion(19) = -HK32*(HK12*P(6,19) + HK13*P(19,22) - HK13*P(4,19) + HK14*P(2,19) + HK15*P(0,19) + HK16*P(1,19) - HK17*P(3,19) - HK9*P(19,23) + HK9*P(5,19));
|
||||
// Kfusion(20) = -HK32*(HK12*P(6,20) + HK13*P(20,22) - HK13*P(4,20) + HK14*P(2,20) + HK15*P(0,20) + HK16*P(1,20) - HK17*P(3,20) - HK9*P(20,23) + HK9*P(5,20));
|
||||
// Kfusion(21) = -HK32*(HK12*P(6,21) + HK13*P(21,22) - HK13*P(4,21) + HK14*P(2,21) + HK15*P(0,21) + HK16*P(1,21) - HK17*P(3,21) - HK9*P(21,23) + HK9*P(5,21));
|
||||
Kfusion(22) = -HK22*HK32;
|
||||
Kfusion(23) = -HK28*HK32;
|
||||
|
||||
sym::ComputeDragYInnovVarAndK(state_vector_prev, P, rho, bcoef_inv, mcoef_corrrected, R_ACC, FLT_EPSILON, &_drag_innov_var(1), &Kfusion);
|
||||
}
|
||||
|
||||
if (_drag_innov_var(axis_index) < R_ACC) {
|
||||
// calculation is badly conditioned
|
||||
return;
|
||||
}
|
||||
|
||||
const float pred_acc = -0.5f * bcoef_inv * rho * rel_wind_body(axis_index) * rel_wind_speed - rel_wind_body(axis_index) * mcoef_corrrected;
|
||||
|
||||
// Apply an innovation consistency check with a 5 Sigma threshold
|
||||
_drag_innov(axis_index) = pred_acc - mea_acc;
|
||||
_drag_test_ratio(axis_index) = sq(_drag_innov(axis_index)) / (sq(5.0f) * _drag_innov_var(axis_index));
|
||||
|
||||
@ -415,6 +415,68 @@ def compute_gnss_yaw_innon_innov_var_and_h(
|
||||
|
||||
return (innov, innov_var, H.T)
|
||||
|
||||
def predict_drag(
|
||||
state: VState,
|
||||
rho: sf.Scalar,
|
||||
cd: sf.Scalar,
|
||||
cm: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
):
|
||||
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
|
||||
|
||||
vel_rel = sf.V3(state[State.vx] - state[State.wx],
|
||||
state[State.vy] - state[State.wy],
|
||||
state[State.vz])
|
||||
vel_rel_body = R_to_body * vel_rel
|
||||
|
||||
bluff_body_drag = -0.5 * rho * cd * sf.V2(vel_rel_body) * vel_rel_body.norm(epsilon=epsilon)
|
||||
momentum_drag = -cm * sf.V2(vel_rel_body)
|
||||
|
||||
return bluff_body_drag + momentum_drag
|
||||
|
||||
|
||||
def compute_drag_x_innov_var_and_k(
|
||||
state: VState,
|
||||
P: MState,
|
||||
rho: sf.Scalar,
|
||||
cd: sf.Scalar,
|
||||
cm: sf.Scalar,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, sf.Scalar, VState):
|
||||
|
||||
meas_pred = predict_drag(state, rho, cd, cm, epsilon)
|
||||
Hx = sf.V1(meas_pred[0]).jacobian(state)
|
||||
innov_var = (Hx * P * Hx.T + R)[0,0]
|
||||
Ktotal = P * Hx.T / sf.Max(innov_var, epsilon)
|
||||
K = VState()
|
||||
K[State.wx] = Ktotal[State.wx]
|
||||
K[State.wy] = Ktotal[State.wy]
|
||||
|
||||
return (innov_var, K)
|
||||
|
||||
def compute_drag_y_innov_var_and_k(
|
||||
state: VState,
|
||||
P: MState,
|
||||
rho: sf.Scalar,
|
||||
cd: sf.Scalar,
|
||||
cm: sf.Scalar,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, sf.Scalar, VState):
|
||||
|
||||
meas_pred = predict_drag(state, rho, cd, cm, epsilon)
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state)
|
||||
innov_var = (Hy * P * Hy.T + R)[0,0]
|
||||
Ktotal = P * Hy.T / sf.Max(innov_var, epsilon)
|
||||
K = VState()
|
||||
K[State.wx] = Ktotal[State.wx]
|
||||
K[State.wy] = Ktotal[State.wy]
|
||||
|
||||
return (innov_var, K)
|
||||
|
||||
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"])
|
||||
@ -433,3 +495,5 @@ generate_px4_function(compute_mag_declination_innov_innov_var_and_h, output_name
|
||||
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"])
|
||||
generate_px4_function(compute_gnss_yaw_innon_innov_var_and_h, output_names=["innov", "innov_var", "H"])
|
||||
generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"])
|
||||
generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"])
|
||||
|
||||
@ -0,0 +1,184 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// 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_drag_x_innov_var_and_k
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* rho: Scalar
|
||||
* cd: Scalar
|
||||
* cm: Scalar
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* K: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar rho,
|
||||
const Scalar cd, const Scalar cm, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
|
||||
// Total ops: 407
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (76)
|
||||
const Scalar _tmp0 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp1 = 2 * _tmp0;
|
||||
const Scalar _tmp2 = _tmp1 * state(1, 0);
|
||||
const Scalar _tmp3 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp4 = 2 * _tmp3;
|
||||
const Scalar _tmp5 = _tmp4 * state(2, 0);
|
||||
const Scalar _tmp6 = 2 * state(6, 0);
|
||||
const Scalar _tmp7 = _tmp6 * state(0, 0);
|
||||
const Scalar _tmp8 = _tmp2 - _tmp5 - _tmp7;
|
||||
const Scalar _tmp9 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp10 = -_tmp9;
|
||||
const Scalar _tmp11 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp12 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp13 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp14 = -_tmp12 + _tmp13;
|
||||
const Scalar _tmp15 = _tmp10 + _tmp11 + _tmp14;
|
||||
const Scalar _tmp16 = state(0, 0) * state(3, 0);
|
||||
const Scalar _tmp17 = state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp18 = _tmp16 + _tmp17;
|
||||
const Scalar _tmp19 = state(0, 0) * state(2, 0);
|
||||
const Scalar _tmp20 = state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp21 = _tmp1 * _tmp18 + _tmp15 * _tmp3 + _tmp6 * (-_tmp19 + _tmp20);
|
||||
const Scalar _tmp22 = state(2, 0) * state(3, 0);
|
||||
const Scalar _tmp23 = state(0, 0) * state(1, 0);
|
||||
const Scalar _tmp24 = _tmp22 - _tmp23;
|
||||
const Scalar _tmp25 = _tmp19 + _tmp20;
|
||||
const Scalar _tmp26 = -_tmp11;
|
||||
const Scalar _tmp27 = _tmp10 + _tmp12 + _tmp13 + _tmp26;
|
||||
const Scalar _tmp28 = _tmp1 * _tmp24 + _tmp25 * _tmp4 + _tmp27 * state(6, 0);
|
||||
const Scalar _tmp29 = _tmp14 + _tmp26 + _tmp9;
|
||||
const Scalar _tmp30 = -_tmp16 + _tmp17;
|
||||
const Scalar _tmp31 = _tmp0 * _tmp29 + _tmp30 * _tmp4 + _tmp6 * (_tmp22 + _tmp23);
|
||||
const Scalar _tmp32 = std::sqrt(Scalar(std::pow(_tmp21, Scalar(2)) + std::pow(_tmp28, Scalar(2)) +
|
||||
std::pow(_tmp31, Scalar(2)) + epsilon));
|
||||
const Scalar _tmp33 = cd * rho;
|
||||
const Scalar _tmp34 = _tmp32 * _tmp33;
|
||||
const Scalar _tmp35 = Scalar(0.5) * _tmp34;
|
||||
const Scalar _tmp36 = _tmp4 * state(0, 0);
|
||||
const Scalar _tmp37 = _tmp1 * state(3, 0);
|
||||
const Scalar _tmp38 = _tmp6 * state(2, 0);
|
||||
const Scalar _tmp39 = _tmp36 + _tmp37 - _tmp38;
|
||||
const Scalar _tmp40 = 2 * _tmp28;
|
||||
const Scalar _tmp41 = _tmp1 * state(2, 0) + _tmp4 * state(1, 0) + _tmp6 * state(3, 0);
|
||||
const Scalar _tmp42 = 2 * _tmp31;
|
||||
const Scalar _tmp43 = 2 * _tmp21;
|
||||
const Scalar _tmp44 = Scalar(0.25) * _tmp21 * _tmp33 / _tmp32;
|
||||
const Scalar _tmp45 =
|
||||
-_tmp35 * _tmp8 - _tmp44 * (_tmp39 * _tmp40 + _tmp41 * _tmp42 + _tmp43 * _tmp8) - _tmp8 * cm;
|
||||
const Scalar _tmp46 = 2 * _tmp19;
|
||||
const Scalar _tmp47 = 2 * _tmp20;
|
||||
const Scalar _tmp48 = -_tmp46 + _tmp47;
|
||||
const Scalar _tmp49 = 2 * _tmp22;
|
||||
const Scalar _tmp50 = 2 * _tmp23;
|
||||
const Scalar _tmp51 = -_tmp35 * _tmp48 -
|
||||
_tmp44 * (_tmp27 * _tmp40 + _tmp42 * (_tmp49 + _tmp50) + _tmp43 * _tmp48) -
|
||||
_tmp48 * cm;
|
||||
const Scalar _tmp52 = -_tmp2 + _tmp5 + _tmp7;
|
||||
const Scalar _tmp53 = _tmp4 * state(3, 0);
|
||||
const Scalar _tmp54 = _tmp1 * state(0, 0);
|
||||
const Scalar _tmp55 = _tmp6 * state(1, 0);
|
||||
const Scalar _tmp56 = -_tmp53 + _tmp54 + _tmp55;
|
||||
const Scalar _tmp57 = -_tmp35 * _tmp39 - _tmp39 * cm -
|
||||
_tmp44 * (_tmp39 * _tmp43 + _tmp40 * _tmp52 + _tmp42 * _tmp56);
|
||||
const Scalar _tmp58 = _tmp15 * cm;
|
||||
const Scalar _tmp59 = _tmp15 * _tmp35;
|
||||
const Scalar _tmp60 = _tmp15 * _tmp43;
|
||||
const Scalar _tmp61 = 2 * _tmp16;
|
||||
const Scalar _tmp62 = 2 * _tmp17;
|
||||
const Scalar _tmp63 =
|
||||
-_tmp44 * (_tmp40 * (_tmp46 + _tmp47) + _tmp42 * (-_tmp61 + _tmp62) + _tmp60) - _tmp58 -
|
||||
_tmp59;
|
||||
const Scalar _tmp64 = 4 * _tmp28;
|
||||
const Scalar _tmp65 = _tmp29 * _tmp42;
|
||||
const Scalar _tmp66 = Scalar(1.0) * _tmp18 * _tmp34 + 2 * _tmp18 * cm -
|
||||
_tmp44 * (-4 * _tmp18 * _tmp21 - _tmp24 * _tmp64 - _tmp65);
|
||||
const Scalar _tmp67 =
|
||||
-_tmp35 * _tmp41 - _tmp41 * cm -
|
||||
_tmp44 * (_tmp40 * (_tmp53 - _tmp54 - _tmp55) + _tmp41 * _tmp43 + _tmp42 * _tmp52);
|
||||
const Scalar _tmp68 = _tmp61 + _tmp62;
|
||||
const Scalar _tmp69 = -_tmp35 * _tmp68 -
|
||||
_tmp44 * (_tmp40 * (_tmp49 - _tmp50) + _tmp43 * _tmp68 + _tmp65) -
|
||||
_tmp68 * cm;
|
||||
const Scalar _tmp70 =
|
||||
-_tmp35 * _tmp56 -
|
||||
_tmp44 * (_tmp40 * _tmp41 + _tmp42 * (-_tmp36 - _tmp37 + _tmp38) + _tmp43 * _tmp56) -
|
||||
_tmp56 * cm;
|
||||
const Scalar _tmp71 =
|
||||
-_tmp44 * (-_tmp25 * _tmp64 - 4 * _tmp30 * _tmp31 - _tmp60) + _tmp58 + _tmp59;
|
||||
const Scalar _tmp72 = P(23, 23) * _tmp66;
|
||||
const Scalar _tmp73 = P(22, 22) * _tmp71;
|
||||
const Scalar _tmp74 = R +
|
||||
_tmp45 * (P(0, 2) * _tmp57 + P(1, 2) * _tmp67 + P(2, 2) * _tmp45 +
|
||||
P(22, 2) * _tmp71 + P(23, 2) * _tmp66 + P(3, 2) * _tmp70 +
|
||||
P(4, 2) * _tmp63 + P(5, 2) * _tmp69 + P(6, 2) * _tmp51) +
|
||||
_tmp51 * (P(0, 6) * _tmp57 + P(1, 6) * _tmp67 + P(2, 6) * _tmp45 +
|
||||
P(22, 6) * _tmp71 + P(23, 6) * _tmp66 + P(3, 6) * _tmp70 +
|
||||
P(4, 6) * _tmp63 + P(5, 6) * _tmp69 + P(6, 6) * _tmp51) +
|
||||
_tmp57 * (P(0, 0) * _tmp57 + P(1, 0) * _tmp67 + P(2, 0) * _tmp45 +
|
||||
P(22, 0) * _tmp71 + P(23, 0) * _tmp66 + P(3, 0) * _tmp70 +
|
||||
P(4, 0) * _tmp63 + P(5, 0) * _tmp69 + P(6, 0) * _tmp51) +
|
||||
_tmp63 * (P(0, 4) * _tmp57 + P(1, 4) * _tmp67 + P(2, 4) * _tmp45 +
|
||||
P(22, 4) * _tmp71 + P(23, 4) * _tmp66 + P(3, 4) * _tmp70 +
|
||||
P(4, 4) * _tmp63 + P(5, 4) * _tmp69 + P(6, 4) * _tmp51) +
|
||||
_tmp66 * (P(0, 23) * _tmp57 + P(1, 23) * _tmp67 + P(2, 23) * _tmp45 +
|
||||
P(22, 23) * _tmp71 + P(3, 23) * _tmp70 + P(4, 23) * _tmp63 +
|
||||
P(5, 23) * _tmp69 + P(6, 23) * _tmp51 + _tmp72) +
|
||||
_tmp67 * (P(0, 1) * _tmp57 + P(1, 1) * _tmp67 + P(2, 1) * _tmp45 +
|
||||
P(22, 1) * _tmp71 + P(23, 1) * _tmp66 + P(3, 1) * _tmp70 +
|
||||
P(4, 1) * _tmp63 + P(5, 1) * _tmp69 + P(6, 1) * _tmp51) +
|
||||
_tmp69 * (P(0, 5) * _tmp57 + P(1, 5) * _tmp67 + P(2, 5) * _tmp45 +
|
||||
P(22, 5) * _tmp71 + P(23, 5) * _tmp66 + P(3, 5) * _tmp70 +
|
||||
P(4, 5) * _tmp63 + P(5, 5) * _tmp69 + P(6, 5) * _tmp51) +
|
||||
_tmp70 * (P(0, 3) * _tmp57 + P(1, 3) * _tmp67 + P(2, 3) * _tmp45 +
|
||||
P(22, 3) * _tmp71 + P(23, 3) * _tmp66 + P(3, 3) * _tmp70 +
|
||||
P(4, 3) * _tmp63 + P(5, 3) * _tmp69 + P(6, 3) * _tmp51) +
|
||||
_tmp71 * (P(0, 22) * _tmp57 + P(1, 22) * _tmp67 + P(2, 22) * _tmp45 +
|
||||
P(23, 22) * _tmp66 + P(3, 22) * _tmp70 + P(4, 22) * _tmp63 +
|
||||
P(5, 22) * _tmp69 + P(6, 22) * _tmp51 + _tmp73);
|
||||
const Scalar _tmp75 = Scalar(1.0) / (math::max<Scalar>(_tmp74, epsilon));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = _tmp74;
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _K = (*K);
|
||||
|
||||
_K.setZero();
|
||||
|
||||
_K(22, 0) = _tmp75 * (P(22, 0) * _tmp57 + P(22, 1) * _tmp67 + P(22, 2) * _tmp45 +
|
||||
P(22, 23) * _tmp66 + P(22, 3) * _tmp70 + P(22, 4) * _tmp63 +
|
||||
P(22, 5) * _tmp69 + P(22, 6) * _tmp51 + _tmp73);
|
||||
_K(23, 0) = _tmp75 * (P(23, 0) * _tmp57 + P(23, 1) * _tmp67 + P(23, 2) * _tmp45 +
|
||||
P(23, 22) * _tmp71 + P(23, 3) * _tmp70 + P(23, 4) * _tmp63 +
|
||||
P(23, 5) * _tmp69 + P(23, 6) * _tmp51 + _tmp72);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
@ -0,0 +1,185 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// 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_drag_y_innov_var_and_k
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* rho: Scalar
|
||||
* cd: Scalar
|
||||
* cm: Scalar
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* K: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeDragYInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar rho,
|
||||
const Scalar cd, const Scalar cm, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
|
||||
// Total ops: 407
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (76)
|
||||
const Scalar _tmp0 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp1 = 2 * _tmp0;
|
||||
const Scalar _tmp2 = _tmp1 * state(1, 0);
|
||||
const Scalar _tmp3 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp4 = 2 * _tmp3;
|
||||
const Scalar _tmp5 = _tmp4 * state(2, 0);
|
||||
const Scalar _tmp6 = 2 * state(6, 0);
|
||||
const Scalar _tmp7 = _tmp6 * state(0, 0);
|
||||
const Scalar _tmp8 = -_tmp2 + _tmp5 + _tmp7;
|
||||
const Scalar _tmp9 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp10 = -_tmp9;
|
||||
const Scalar _tmp11 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp12 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp13 = -_tmp12;
|
||||
const Scalar _tmp14 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp15 = _tmp10 + _tmp11 + _tmp13 + _tmp14;
|
||||
const Scalar _tmp16 = state(0, 0) * state(3, 0);
|
||||
const Scalar _tmp17 = state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp18 = _tmp16 + _tmp17;
|
||||
const Scalar _tmp19 = state(0, 0) * state(2, 0);
|
||||
const Scalar _tmp20 = state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp21 = _tmp1 * _tmp18 + _tmp15 * _tmp3 + _tmp6 * (-_tmp19 + _tmp20);
|
||||
const Scalar _tmp22 = state(2, 0) * state(3, 0);
|
||||
const Scalar _tmp23 = state(0, 0) * state(1, 0);
|
||||
const Scalar _tmp24 = _tmp22 - _tmp23;
|
||||
const Scalar _tmp25 = _tmp19 + _tmp20;
|
||||
const Scalar _tmp26 = _tmp11 - _tmp14;
|
||||
const Scalar _tmp27 = _tmp13 + _tmp26 + _tmp9;
|
||||
const Scalar _tmp28 = _tmp1 * _tmp24 + _tmp25 * _tmp4 + _tmp27 * state(6, 0);
|
||||
const Scalar _tmp29 = _tmp10 + _tmp12 + _tmp26;
|
||||
const Scalar _tmp30 = -_tmp16 + _tmp17;
|
||||
const Scalar _tmp31 = _tmp0 * _tmp29 + _tmp30 * _tmp4 + _tmp6 * (_tmp22 + _tmp23);
|
||||
const Scalar _tmp32 = std::sqrt(Scalar(std::pow(_tmp21, Scalar(2)) + std::pow(_tmp28, Scalar(2)) +
|
||||
std::pow(_tmp31, Scalar(2)) + epsilon));
|
||||
const Scalar _tmp33 = cd * rho;
|
||||
const Scalar _tmp34 = _tmp32 * _tmp33;
|
||||
const Scalar _tmp35 = Scalar(0.5) * _tmp34;
|
||||
const Scalar _tmp36 = _tmp4 * state(3, 0);
|
||||
const Scalar _tmp37 = _tmp1 * state(0, 0);
|
||||
const Scalar _tmp38 = _tmp6 * state(1, 0);
|
||||
const Scalar _tmp39 = 2 * _tmp28;
|
||||
const Scalar _tmp40 = 2 * _tmp31;
|
||||
const Scalar _tmp41 = _tmp1 * state(2, 0) + _tmp4 * state(1, 0) + _tmp6 * state(3, 0);
|
||||
const Scalar _tmp42 = 2 * _tmp21;
|
||||
const Scalar _tmp43 = Scalar(0.25) * _tmp31 * _tmp33 / _tmp32;
|
||||
const Scalar _tmp44 =
|
||||
-_tmp35 * _tmp8 -
|
||||
_tmp43 * (_tmp39 * (_tmp36 - _tmp37 - _tmp38) + _tmp40 * _tmp8 + _tmp41 * _tmp42) -
|
||||
_tmp8 * cm;
|
||||
const Scalar _tmp45 = -_tmp36 + _tmp37 + _tmp38;
|
||||
const Scalar _tmp46 = _tmp4 * state(0, 0);
|
||||
const Scalar _tmp47 = _tmp1 * state(3, 0);
|
||||
const Scalar _tmp48 = _tmp6 * state(2, 0);
|
||||
const Scalar _tmp49 = _tmp46 + _tmp47 - _tmp48;
|
||||
const Scalar _tmp50 = -_tmp35 * _tmp45 -
|
||||
_tmp43 * (_tmp39 * _tmp8 + _tmp40 * _tmp45 + _tmp42 * _tmp49) - _tmp45 * cm;
|
||||
const Scalar _tmp51 =
|
||||
-_tmp35 * _tmp41 - _tmp41 * cm -
|
||||
_tmp43 * (_tmp39 * _tmp49 + _tmp40 * _tmp41 + _tmp42 * (_tmp2 - _tmp5 - _tmp7));
|
||||
const Scalar _tmp52 = -_tmp46 - _tmp47 + _tmp48;
|
||||
const Scalar _tmp53 = -_tmp35 * _tmp52 -
|
||||
_tmp43 * (_tmp39 * _tmp41 + _tmp40 * _tmp52 + _tmp42 * _tmp45) -
|
||||
_tmp52 * cm;
|
||||
const Scalar _tmp54 = _tmp29 * cm;
|
||||
const Scalar _tmp55 = _tmp29 * _tmp35;
|
||||
const Scalar _tmp56 = 4 * _tmp28;
|
||||
const Scalar _tmp57 = _tmp29 * _tmp40;
|
||||
const Scalar _tmp58 =
|
||||
-_tmp43 * (-4 * _tmp18 * _tmp21 - _tmp24 * _tmp56 - _tmp57) + _tmp54 + _tmp55;
|
||||
const Scalar _tmp59 = 2 * _tmp16;
|
||||
const Scalar _tmp60 = 2 * _tmp17;
|
||||
const Scalar _tmp61 = -_tmp59 + _tmp60;
|
||||
const Scalar _tmp62 = _tmp15 * _tmp42;
|
||||
const Scalar _tmp63 = 2 * _tmp19;
|
||||
const Scalar _tmp64 = 2 * _tmp20;
|
||||
const Scalar _tmp65 = -_tmp35 * _tmp61 -
|
||||
_tmp43 * (_tmp39 * (_tmp63 + _tmp64) + _tmp40 * _tmp61 + _tmp62) -
|
||||
_tmp61 * cm;
|
||||
const Scalar _tmp66 = Scalar(1.0) * _tmp30 * _tmp34 + 2 * _tmp30 * cm -
|
||||
_tmp43 * (-_tmp25 * _tmp56 - 4 * _tmp30 * _tmp31 - _tmp62);
|
||||
const Scalar _tmp67 = 2 * _tmp22;
|
||||
const Scalar _tmp68 = 2 * _tmp23;
|
||||
const Scalar _tmp69 = _tmp67 + _tmp68;
|
||||
const Scalar _tmp70 = -_tmp35 * _tmp69 -
|
||||
_tmp43 * (_tmp27 * _tmp39 + _tmp40 * _tmp69 + _tmp42 * (-_tmp63 + _tmp64)) -
|
||||
_tmp69 * cm;
|
||||
const Scalar _tmp71 =
|
||||
-_tmp43 * (_tmp39 * (_tmp67 - _tmp68) + _tmp42 * (_tmp59 + _tmp60) + _tmp57) - _tmp54 -
|
||||
_tmp55;
|
||||
const Scalar _tmp72 = P(22, 22) * _tmp66;
|
||||
const Scalar _tmp73 = P(23, 23) * _tmp58;
|
||||
const Scalar _tmp74 = R +
|
||||
_tmp44 * (P(0, 1) * _tmp50 + P(1, 1) * _tmp44 + P(2, 1) * _tmp51 +
|
||||
P(22, 1) * _tmp66 + P(23, 1) * _tmp58 + P(3, 1) * _tmp53 +
|
||||
P(4, 1) * _tmp65 + P(5, 1) * _tmp71 + P(6, 1) * _tmp70) +
|
||||
_tmp50 * (P(0, 0) * _tmp50 + P(1, 0) * _tmp44 + P(2, 0) * _tmp51 +
|
||||
P(22, 0) * _tmp66 + P(23, 0) * _tmp58 + P(3, 0) * _tmp53 +
|
||||
P(4, 0) * _tmp65 + P(5, 0) * _tmp71 + P(6, 0) * _tmp70) +
|
||||
_tmp51 * (P(0, 2) * _tmp50 + P(1, 2) * _tmp44 + P(2, 2) * _tmp51 +
|
||||
P(22, 2) * _tmp66 + P(23, 2) * _tmp58 + P(3, 2) * _tmp53 +
|
||||
P(4, 2) * _tmp65 + P(5, 2) * _tmp71 + P(6, 2) * _tmp70) +
|
||||
_tmp53 * (P(0, 3) * _tmp50 + P(1, 3) * _tmp44 + P(2, 3) * _tmp51 +
|
||||
P(22, 3) * _tmp66 + P(23, 3) * _tmp58 + P(3, 3) * _tmp53 +
|
||||
P(4, 3) * _tmp65 + P(5, 3) * _tmp71 + P(6, 3) * _tmp70) +
|
||||
_tmp58 * (P(0, 23) * _tmp50 + P(1, 23) * _tmp44 + P(2, 23) * _tmp51 +
|
||||
P(22, 23) * _tmp66 + P(3, 23) * _tmp53 + P(4, 23) * _tmp65 +
|
||||
P(5, 23) * _tmp71 + P(6, 23) * _tmp70 + _tmp73) +
|
||||
_tmp65 * (P(0, 4) * _tmp50 + P(1, 4) * _tmp44 + P(2, 4) * _tmp51 +
|
||||
P(22, 4) * _tmp66 + P(23, 4) * _tmp58 + P(3, 4) * _tmp53 +
|
||||
P(4, 4) * _tmp65 + P(5, 4) * _tmp71 + P(6, 4) * _tmp70) +
|
||||
_tmp66 * (P(0, 22) * _tmp50 + P(1, 22) * _tmp44 + P(2, 22) * _tmp51 +
|
||||
P(23, 22) * _tmp58 + P(3, 22) * _tmp53 + P(4, 22) * _tmp65 +
|
||||
P(5, 22) * _tmp71 + P(6, 22) * _tmp70 + _tmp72) +
|
||||
_tmp70 * (P(0, 6) * _tmp50 + P(1, 6) * _tmp44 + P(2, 6) * _tmp51 +
|
||||
P(22, 6) * _tmp66 + P(23, 6) * _tmp58 + P(3, 6) * _tmp53 +
|
||||
P(4, 6) * _tmp65 + P(5, 6) * _tmp71 + P(6, 6) * _tmp70) +
|
||||
_tmp71 * (P(0, 5) * _tmp50 + P(1, 5) * _tmp44 + P(2, 5) * _tmp51 +
|
||||
P(22, 5) * _tmp66 + P(23, 5) * _tmp58 + P(3, 5) * _tmp53 +
|
||||
P(4, 5) * _tmp65 + P(5, 5) * _tmp71 + P(6, 5) * _tmp70);
|
||||
const Scalar _tmp75 = Scalar(1.0) / (math::max<Scalar>(_tmp74, epsilon));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = _tmp74;
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _K = (*K);
|
||||
|
||||
_K.setZero();
|
||||
|
||||
_K(22, 0) = _tmp75 * (P(22, 0) * _tmp50 + P(22, 1) * _tmp44 + P(22, 2) * _tmp51 +
|
||||
P(22, 23) * _tmp58 + P(22, 3) * _tmp53 + P(22, 4) * _tmp65 +
|
||||
P(22, 5) * _tmp71 + P(22, 6) * _tmp70 + _tmp72);
|
||||
_K(23, 0) = _tmp75 * (P(23, 0) * _tmp50 + P(23, 1) * _tmp44 + P(23, 2) * _tmp51 +
|
||||
P(23, 22) * _tmp66 + P(23, 3) * _tmp53 + P(23, 4) * _tmp65 +
|
||||
P(23, 5) * _tmp71 + P(23, 6) * _tmp70 + _tmp73);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
Loading…
x
Reference in New Issue
Block a user