mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
ekf2: migrate fuse_airspeed to SymForce
This commit is contained in:
bresch
Daniel Agar
parent
41cda14126
commit
29ebef1f74
@ -43,6 +43,10 @@
|
||||
*/
|
||||
|
||||
#include "ekf.h"
|
||||
|
||||
#include "python/ekf_derivation/generated/compute_airspeed_h_and_k.h"
|
||||
#include "python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h"
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source_1d_s &airspeed) const
|
||||
@ -50,38 +54,17 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so
|
||||
// reset flags
|
||||
resetEstimatorAidStatusFlags(airspeed);
|
||||
|
||||
const float vn = _state.vel(0); // Velocity in north direction
|
||||
const float ve = _state.vel(1); // Velocity in east direction
|
||||
const float vd = _state.vel(2); // Velocity in downwards direction
|
||||
const float vwn = _state.wind_vel(0); // Wind speed in north direction
|
||||
const float vwe = _state.wind_vel(1); // Wind speed in east direction
|
||||
|
||||
// Variance for true airspeed measurement - (m/sec)^2
|
||||
const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
|
||||
math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
|
||||
const float R = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
|
||||
math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
|
||||
|
||||
// Intermediate variables
|
||||
const float IV0 = ve - vwe;
|
||||
const float IV1 = vn - vwn;
|
||||
const float IV2 = (IV0)*(IV0) + (IV1)*(IV1) + (vd)*(vd);
|
||||
|
||||
const float predicted_airspeed = sqrtf(IV2);
|
||||
|
||||
if (fabsf(predicted_airspeed) < FLT_EPSILON) {
|
||||
return;
|
||||
}
|
||||
|
||||
const float IV3 = 1.0F/(IV2);
|
||||
const float IV4 = IV0*P(5,23);
|
||||
const float IV5 = IV0*IV3;
|
||||
const float IV6 = IV1*P(4,22);
|
||||
const float IV7 = IV1*IV3;
|
||||
|
||||
const float innov_var = IV3*vd*(IV0*P(5,6) - IV0*P(6,23) + IV1*P(4,6) - IV1*P(6,22) + P(6,6)*vd) - IV5*(-IV0*P(23,23) - IV1*P(22,23) + IV1*P(4,23) + IV4 + P(6,23)*vd) + IV5*(IV0*P(5,5) + IV1*P(4,5) - IV1*P(5,22) - IV4 + P(5,6)*vd) - IV7*(-IV0*P(22,23) + IV0*P(5,22) - IV1*P(22,22) + IV6 + P(6,22)*vd) + IV7*(-IV0*P(4,23) + IV0*P(4,5) + IV1*P(4,4) - IV6 + P(4,6)*vd) + R_TAS;
|
||||
float innov = 0.f;
|
||||
float innov_var = 0.f;
|
||||
sym::ComputeAirspeedInnovAndInnovVar(getStateAtFusionHorizonAsVector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, &innov, &innov_var);
|
||||
|
||||
airspeed.observation = airspeed_sample.true_airspeed;
|
||||
airspeed.observation_variance = R_TAS;
|
||||
airspeed.innovation = predicted_airspeed - airspeed.observation;
|
||||
airspeed.observation_variance = R;
|
||||
airspeed.innovation = innov;
|
||||
airspeed.innovation_variance = innov_var;
|
||||
|
||||
airspeed.fusion_enabled = _control_status.flags.fuse_aspd;
|
||||
@ -98,34 +81,9 @@ void Ekf::fuseAirspeed(estimator_aid_source_1d_s &airspeed)
|
||||
return;
|
||||
}
|
||||
|
||||
const float vn = _state.vel(0); // Velocity in north direction
|
||||
const float ve = _state.vel(1); // Velocity in east direction
|
||||
const float vd = _state.vel(2); // Velocity in downwards direction
|
||||
const float vwn = _state.wind_vel(0); // Wind speed in north direction
|
||||
const float vwe = _state.wind_vel(1); // Wind speed in east direction
|
||||
|
||||
// determine if we need the airspeed fusion to correct states other than wind
|
||||
const bool update_wind_only = !_control_status.flags.wind_dead_reckoning;
|
||||
|
||||
// Intermediate variables
|
||||
const float HK0 = vn - vwn;
|
||||
const float HK1 = ve - vwe;
|
||||
const float HK2 = sqrtf((HK0)*(HK0) + (HK1)*(HK1) + (vd)*(vd));
|
||||
|
||||
const float predicted_airspeed = HK2;
|
||||
|
||||
if (predicted_airspeed < 1.0f) {
|
||||
// calculation can be badly conditioned for very low airspeed values so don't fuse this time
|
||||
return;
|
||||
}
|
||||
|
||||
const float HK3 = 1.0F/(HK2);
|
||||
const float HK4 = HK0*HK3;
|
||||
const float HK5 = HK1*HK3;
|
||||
const float HK6 = HK3*vd;
|
||||
const float HK7 = -HK0*HK3;
|
||||
const float HK8 = -HK1*HK3;
|
||||
|
||||
const float innov_var = airspeed.innovation_variance;
|
||||
|
||||
if (innov_var < airspeed.observation_variance || innov_var < FLT_EPSILON) {
|
||||
@ -150,31 +108,22 @@ void Ekf::fuseAirspeed(estimator_aid_source_1d_s &airspeed)
|
||||
return;
|
||||
}
|
||||
|
||||
const float HK9 = 1.0F/(innov_var);
|
||||
|
||||
_fault_status.flags.bad_airspeed = false;
|
||||
|
||||
// Observation Jacobians
|
||||
SparseVector24f<4,5,6,22,23> Hfusion;
|
||||
Hfusion.at<4>() = HK4;
|
||||
Hfusion.at<5>() = HK5;
|
||||
Hfusion.at<6>() = HK6;
|
||||
Hfusion.at<22>() = HK7;
|
||||
Hfusion.at<23>() = HK8;
|
||||
Vector24f H; // Observation jacobian
|
||||
Vector24f K; // Kalman gain vector
|
||||
|
||||
Vector24f Kfusion; // Kalman gain vector
|
||||
sym::ComputeAirspeedHAndK(getStateAtFusionHorizonAsVector(), P, innov_var, FLT_EPSILON, &H, &K);
|
||||
|
||||
if (!update_wind_only) {
|
||||
// we have no other source of aiding, so use airspeed measurements to correct states
|
||||
SparseVector24f<4,5,6,22,23> H_sparse(H);
|
||||
|
||||
if (update_wind_only) {
|
||||
for (unsigned row = 0; row <= 21; row++) {
|
||||
Kfusion(row) = HK9*(HK4*P(row,4) + HK5*P(row,5) + HK6*P(row,6) + HK7*P(row,22) + HK8*P(row,23));
|
||||
K(row) = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
Kfusion(22) = HK9*(HK4*P(4,22) + HK5*P(5,22) + HK6*P(6,22) + HK7*P(22,22) + HK8*P(22,23));
|
||||
Kfusion(23) = HK9*(HK4*P(4,23) + HK5*P(5,23) + HK6*P(6,23) + HK7*P(22,23) + HK8*P(23,23));
|
||||
|
||||
const bool is_fused = measurementUpdate(Kfusion, Hfusion, airspeed.innovation);
|
||||
const bool is_fused = measurementUpdate(K, H_sparse, airspeed.innovation);
|
||||
|
||||
airspeed.fused = is_fused;
|
||||
_fault_status.flags.bad_airspeed = !is_fused;
|
||||
|
||||
106
src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
Normal file
106
src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
Normal file
@ -0,0 +1,106 @@
|
||||
#!/usr/bin/env python
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Copyright (c) 2022 PX4 Development Team
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions
|
||||
are met:
|
||||
|
||||
1. Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
2. Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in
|
||||
the documentation and/or other materials provided with the
|
||||
distribution.
|
||||
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.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
File: derivation.py
|
||||
Description:
|
||||
"""
|
||||
|
||||
import symforce.symbolic as sf
|
||||
from derivation_utils import *
|
||||
|
||||
class State:
|
||||
qw = 0
|
||||
qx = 1
|
||||
qy = 2
|
||||
qz = 3
|
||||
vx = 4
|
||||
vy = 5
|
||||
vz = 6
|
||||
px = 7
|
||||
py = 8
|
||||
pz = 9
|
||||
d_ang_bx = 10
|
||||
d_ang_by = 11
|
||||
d_ang_bz = 12
|
||||
d_vel_bx = 13
|
||||
d_vel_by = 14
|
||||
d_vel_bz = 15
|
||||
ix = 16
|
||||
iy = 17
|
||||
iz = 18
|
||||
ibx = 19
|
||||
iby = 20
|
||||
ibz = 21
|
||||
wx = 22
|
||||
wy = 23
|
||||
n_states = 24
|
||||
|
||||
class VState(sf.Matrix):
|
||||
SHAPE = (State.n_states, 1)
|
||||
|
||||
class MState(sf.Matrix):
|
||||
SHAPE = (State.n_states, State.n_states)
|
||||
|
||||
def compute_airspeed_innov_and_innov_var(
|
||||
state: VState,
|
||||
P: MState,
|
||||
airspeed: sf.Scalar,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, sf.Scalar):
|
||||
|
||||
vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz])
|
||||
airspeed_pred = vel_rel.norm(epsilon=epsilon)
|
||||
|
||||
innov = airspeed_pred - airspeed
|
||||
|
||||
H = sf.V1(airspeed_pred).jacobian(state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov, innov_var)
|
||||
|
||||
def compute_airspeed_h_and_k(
|
||||
state: VState,
|
||||
P: MState,
|
||||
innov_var: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (VState, VState):
|
||||
|
||||
vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz])
|
||||
airspeed_pred = vel_rel.norm(epsilon=epsilon)
|
||||
H = sf.V1(airspeed_pred).jacobian(state)
|
||||
|
||||
K = P * H.T / sm.Max(innov_var, epsilon)
|
||||
|
||||
return (H.T, K)
|
||||
|
||||
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"])
|
||||
101
src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py
Normal file
101
src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py
Normal file
@ -0,0 +1,101 @@
|
||||
#!/usr/bin/env python
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Copyright (c) 2022 PX4 Development Team
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions
|
||||
are met:
|
||||
|
||||
1. Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
2. Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in
|
||||
the documentation and/or other materials provided with the
|
||||
distribution.
|
||||
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.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
File: derivation_utils.py
|
||||
Description:
|
||||
Common functions used for the derivation of most estimators
|
||||
"""
|
||||
|
||||
from symforce import symbolic as sm
|
||||
from symforce import geo
|
||||
from symforce import typing as T
|
||||
|
||||
# q: quaternion describing rotation from frame 1 to frame 2
|
||||
# returns a rotation matrix derived form q which describes the same
|
||||
# rotation
|
||||
def quat_to_rot(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):
|
||||
# Avoids a singularity at 0 while keeping the derivative correct
|
||||
return expr.subs(var, var + eps * sign_no_zero(var))
|
||||
|
||||
def generate_px4_function(function_name, output_names):
|
||||
from symforce.codegen import Codegen, CppConfig
|
||||
import os
|
||||
import fileinput
|
||||
|
||||
codegen = Codegen.function(
|
||||
function_name,
|
||||
output_names=output_names,
|
||||
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
|
||||
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='')
|
||||
|
||||
def generate_python_function(function_name, output_names):
|
||||
from symforce.codegen import Codegen, PythonConfig
|
||||
codegen = Codegen.function(
|
||||
function_name,
|
||||
output_names=output_names,
|
||||
config=PythonConfig())
|
||||
|
||||
metadata = codegen.generate_function(
|
||||
output_dir="generated",
|
||||
skip_directory_nesting=True)
|
||||
@ -0,0 +1,116 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// 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_airspeed_h_and_k
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_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, 24, 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(4, 0) = _tmp3;
|
||||
_H(5, 0) = _tmp4;
|
||||
_H(6, 0) = _tmp5;
|
||||
_H(22, 0) = -_tmp3;
|
||||
_H(23, 0) = -_tmp4;
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _K = (*K);
|
||||
|
||||
_K(0, 0) = _tmp6 * (-P(0, 22) * _tmp3 - P(0, 23) * _tmp4 + P(0, 4) * _tmp3 + P(0, 5) * _tmp4 +
|
||||
P(0, 6) * _tmp5);
|
||||
_K(1, 0) = _tmp6 * (-P(1, 22) * _tmp3 - P(1, 23) * _tmp4 + P(1, 4) * _tmp3 + P(1, 5) * _tmp4 +
|
||||
P(1, 6) * _tmp5);
|
||||
_K(2, 0) = _tmp6 * (-P(2, 22) * _tmp3 - P(2, 23) * _tmp4 + P(2, 4) * _tmp3 + P(2, 5) * _tmp4 +
|
||||
P(2, 6) * _tmp5);
|
||||
_K(3, 0) = _tmp6 * (-P(3, 22) * _tmp3 - P(3, 23) * _tmp4 + P(3, 4) * _tmp3 + P(3, 5) * _tmp4 +
|
||||
P(3, 6) * _tmp5);
|
||||
_K(4, 0) = _tmp6 * (-P(4, 22) * _tmp3 - P(4, 23) * _tmp4 + P(4, 4) * _tmp3 + P(4, 5) * _tmp4 +
|
||||
P(4, 6) * _tmp5);
|
||||
_K(5, 0) = _tmp6 * (-P(5, 22) * _tmp3 - P(5, 23) * _tmp4 + P(5, 4) * _tmp3 + P(5, 5) * _tmp4 +
|
||||
P(5, 6) * _tmp5);
|
||||
_K(6, 0) = _tmp6 * (-P(6, 22) * _tmp3 - P(6, 23) * _tmp4 + P(6, 4) * _tmp3 + P(6, 5) * _tmp4 +
|
||||
P(6, 6) * _tmp5);
|
||||
_K(7, 0) = _tmp6 * (-P(7, 22) * _tmp3 - P(7, 23) * _tmp4 + P(7, 4) * _tmp3 + P(7, 5) * _tmp4 +
|
||||
P(7, 6) * _tmp5);
|
||||
_K(8, 0) = _tmp6 * (-P(8, 22) * _tmp3 - P(8, 23) * _tmp4 + P(8, 4) * _tmp3 + P(8, 5) * _tmp4 +
|
||||
P(8, 6) * _tmp5);
|
||||
_K(9, 0) = _tmp6 * (-P(9, 22) * _tmp3 - P(9, 23) * _tmp4 + P(9, 4) * _tmp3 + P(9, 5) * _tmp4 +
|
||||
P(9, 6) * _tmp5);
|
||||
_K(10, 0) = _tmp6 * (-P(10, 22) * _tmp3 - P(10, 23) * _tmp4 + P(10, 4) * _tmp3 +
|
||||
P(10, 5) * _tmp4 + P(10, 6) * _tmp5);
|
||||
_K(11, 0) = _tmp6 * (-P(11, 22) * _tmp3 - P(11, 23) * _tmp4 + P(11, 4) * _tmp3 +
|
||||
P(11, 5) * _tmp4 + P(11, 6) * _tmp5);
|
||||
_K(12, 0) = _tmp6 * (-P(12, 22) * _tmp3 - P(12, 23) * _tmp4 + P(12, 4) * _tmp3 +
|
||||
P(12, 5) * _tmp4 + P(12, 6) * _tmp5);
|
||||
_K(13, 0) = _tmp6 * (-P(13, 22) * _tmp3 - P(13, 23) * _tmp4 + P(13, 4) * _tmp3 +
|
||||
P(13, 5) * _tmp4 + P(13, 6) * _tmp5);
|
||||
_K(14, 0) = _tmp6 * (-P(14, 22) * _tmp3 - P(14, 23) * _tmp4 + P(14, 4) * _tmp3 +
|
||||
P(14, 5) * _tmp4 + P(14, 6) * _tmp5);
|
||||
_K(15, 0) = _tmp6 * (-P(15, 22) * _tmp3 - P(15, 23) * _tmp4 + P(15, 4) * _tmp3 +
|
||||
P(15, 5) * _tmp4 + P(15, 6) * _tmp5);
|
||||
_K(16, 0) = _tmp6 * (-P(16, 22) * _tmp3 - P(16, 23) * _tmp4 + P(16, 4) * _tmp3 +
|
||||
P(16, 5) * _tmp4 + P(16, 6) * _tmp5);
|
||||
_K(17, 0) = _tmp6 * (-P(17, 22) * _tmp3 - P(17, 23) * _tmp4 + P(17, 4) * _tmp3 +
|
||||
P(17, 5) * _tmp4 + P(17, 6) * _tmp5);
|
||||
_K(18, 0) = _tmp6 * (-P(18, 22) * _tmp3 - P(18, 23) * _tmp4 + P(18, 4) * _tmp3 +
|
||||
P(18, 5) * _tmp4 + P(18, 6) * _tmp5);
|
||||
_K(19, 0) = _tmp6 * (-P(19, 22) * _tmp3 - P(19, 23) * _tmp4 + P(19, 4) * _tmp3 +
|
||||
P(19, 5) * _tmp4 + P(19, 6) * _tmp5);
|
||||
_K(20, 0) = _tmp6 * (-P(20, 22) * _tmp3 - P(20, 23) * _tmp4 + P(20, 4) * _tmp3 +
|
||||
P(20, 5) * _tmp4 + P(20, 6) * _tmp5);
|
||||
_K(21, 0) = _tmp6 * (-P(21, 22) * _tmp3 - P(21, 23) * _tmp4 + P(21, 4) * _tmp3 +
|
||||
P(21, 5) * _tmp4 + P(21, 6) * _tmp5);
|
||||
_K(22, 0) = _tmp6 * (-P(22, 22) * _tmp3 - P(22, 23) * _tmp4 + P(22, 4) * _tmp3 +
|
||||
P(22, 5) * _tmp4 + P(22, 6) * _tmp5);
|
||||
_K(23, 0) = _tmp6 * (-P(23, 22) * _tmp3 - P(23, 23) * _tmp4 + P(23, 4) * _tmp3 +
|
||||
P(23, 5) * _tmp4 + P(23, 6) * _tmp5);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
@ -0,0 +1,74 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// 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_airspeed_innov_and_innov_var
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* airspeed: Scalar
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov: Scalar
|
||||
* innov_var: Scalar
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar airspeed,
|
||||
const Scalar R, const Scalar epsilon,
|
||||
Scalar* const innov = nullptr,
|
||||
Scalar* const innov_var = nullptr) {
|
||||
// Total ops: 69
|
||||
|
||||
// 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::sqrt(Scalar(std::pow(_tmp0, Scalar(2)) + std::pow(_tmp1, Scalar(2)) +
|
||||
epsilon + std::pow(state(6, 0), Scalar(2))));
|
||||
const Scalar _tmp3 = Scalar(1.0) / (_tmp2);
|
||||
const Scalar _tmp4 = _tmp3 * state(6, 0);
|
||||
const Scalar _tmp5 = _tmp1 * _tmp3;
|
||||
const Scalar _tmp6 = _tmp0 * _tmp3;
|
||||
|
||||
// Output terms (2)
|
||||
if (innov != nullptr) {
|
||||
Scalar& _innov = (*innov);
|
||||
|
||||
_innov = _tmp2 - airspeed;
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R +
|
||||
_tmp4 * (-P(22, 6) * _tmp5 - P(23, 6) * _tmp6 + P(4, 6) * _tmp5 + P(5, 6) * _tmp6 +
|
||||
P(6, 6) * _tmp4) -
|
||||
_tmp5 * (-P(22, 22) * _tmp5 - P(23, 22) * _tmp6 + P(4, 22) * _tmp5 +
|
||||
P(5, 22) * _tmp6 + P(6, 22) * _tmp4) +
|
||||
_tmp5 * (-P(22, 4) * _tmp5 - P(23, 4) * _tmp6 + P(4, 4) * _tmp5 + P(5, 4) * _tmp6 +
|
||||
P(6, 4) * _tmp4) -
|
||||
_tmp6 * (-P(22, 23) * _tmp5 - P(23, 23) * _tmp6 + P(4, 23) * _tmp5 +
|
||||
P(5, 23) * _tmp6 + P(6, 23) * _tmp4) +
|
||||
_tmp6 * (-P(22, 5) * _tmp5 - P(23, 5) * _tmp6 + P(4, 5) * _tmp5 + P(5, 5) * _tmp6 +
|
||||
P(6, 5) * _tmp4);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
Loading…
x
Reference in New Issue
Block a user