mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-14 23:30:34 +08:00
ekf2: migrate flow fusion to SymForce
This commit is contained in:
bresch
@@ -45,6 +45,8 @@
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <float.h>
|
||||
#include "python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h"
|
||||
#include "python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h"
|
||||
#include "utils.hpp"
|
||||
|
||||
void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
|
||||
@@ -106,17 +108,6 @@ void Ekf::fuseOptFlow()
|
||||
|
||||
const float R_LOS = _aid_src_optical_flow.observation_variance[0];
|
||||
|
||||
// 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);
|
||||
|
||||
// calculate the height above the ground of the optical flow camera. Since earth frame is NED
|
||||
// a positive offset in earth frame leads to a smaller height above the ground.
|
||||
const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
|
||||
@@ -127,129 +118,19 @@ void Ekf::fuseOptFlow()
|
||||
const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
|
||||
const float range = height_above_gnd_est / earth_to_body(2, 2); // absolute distance to the frame region in view
|
||||
|
||||
// The derivation allows for an arbitrary body to flow sensor frame rotation which is
|
||||
// currently not supported by the EKF, so assume sensor frame is aligned with the
|
||||
// body frame
|
||||
const Dcmf Tbs = matrix::eye<float, 3>();
|
||||
const Vector24f state_vector = getStateAtFusionHorizonAsVector();
|
||||
|
||||
// Sub Expressions
|
||||
const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0;
|
||||
const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1;
|
||||
const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2;
|
||||
const float HK3 = HK0*vd + HK1*ve + HK2*vn;
|
||||
const float HK4 = 1.0F/range;
|
||||
const float HK5 = 2*HK4;
|
||||
const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
|
||||
const float HK7 = -HK0*ve + HK1*vd + HK6*vn;
|
||||
const float HK8 = HK0*vn - HK2*vd + HK6*ve;
|
||||
const float HK9 = -HK1*vn + HK2*ve + HK6*vd;
|
||||
const float HK10 = q0*q2;
|
||||
const float HK11 = q1*q3;
|
||||
const float HK12 = HK10 + HK11;
|
||||
const float HK13 = 2*Tbs(1,2);
|
||||
const float HK14 = q0*q3;
|
||||
const float HK15 = q1*q2;
|
||||
const float HK16 = HK14 - HK15;
|
||||
const float HK17 = 2*Tbs(1,1);
|
||||
const float HK18 = ecl::powf(q1, 2);
|
||||
const float HK19 = ecl::powf(q2, 2);
|
||||
const float HK20 = -HK19;
|
||||
const float HK21 = ecl::powf(q0, 2);
|
||||
const float HK22 = ecl::powf(q3, 2);
|
||||
const float HK23 = HK21 - HK22;
|
||||
const float HK24 = HK18 + HK20 + HK23;
|
||||
const float HK25 = HK12*HK13 - HK16*HK17 + HK24*Tbs(1,0);
|
||||
const float HK26 = HK14 + HK15;
|
||||
const float HK27 = 2*Tbs(1,0);
|
||||
const float HK28 = q0*q1;
|
||||
const float HK29 = q2*q3;
|
||||
const float HK30 = HK28 - HK29;
|
||||
const float HK31 = -HK18;
|
||||
const float HK32 = HK19 + HK23 + HK31;
|
||||
const float HK33 = -HK13*HK30 + HK26*HK27 + HK32*Tbs(1,1);
|
||||
const float HK34 = HK28 + HK29;
|
||||
const float HK35 = HK10 - HK11;
|
||||
const float HK36 = HK20 + HK21 + HK22 + HK31;
|
||||
const float HK37 = HK17*HK34 - HK27*HK35 + HK36*Tbs(1,2);
|
||||
const float HK38 = 2*HK3;
|
||||
const float HK39 = 2*HK7;
|
||||
const float HK40 = 2*HK8;
|
||||
const float HK41 = 2*HK9;
|
||||
const float HK42 = HK25*P(0,4) + HK33*P(0,5) + HK37*P(0,6) + HK38*P(0,0) + HK39*P(0,1) + HK40*P(0,2) + HK41*P(0,3);
|
||||
const float HK43 = ecl::powf(range, -2);
|
||||
const float HK44 = HK25*P(4,6) + HK33*P(5,6) + HK37*P(6,6) + HK38*P(0,6) + HK39*P(1,6) + HK40*P(2,6) + HK41*P(3,6);
|
||||
const float HK45 = HK25*P(4,5) + HK33*P(5,5) + HK37*P(5,6) + HK38*P(0,5) + HK39*P(1,5) + HK40*P(2,5) + HK41*P(3,5);
|
||||
const float HK46 = HK25*P(4,4) + HK33*P(4,5) + HK37*P(4,6) + HK38*P(0,4) + HK39*P(1,4) + HK40*P(2,4) + HK41*P(3,4);
|
||||
const float HK47 = HK25*P(2,4) + HK33*P(2,5) + HK37*P(2,6) + HK38*P(0,2) + HK39*P(1,2) + HK40*P(2,2) + HK41*P(2,3);
|
||||
const float HK48 = HK25*P(3,4) + HK33*P(3,5) + HK37*P(3,6) + HK38*P(0,3) + HK39*P(1,3) + HK40*P(2,3) + HK41*P(3,3);
|
||||
const float HK49 = HK25*P(1,4) + HK33*P(1,5) + HK37*P(1,6) + HK38*P(0,1) + HK39*P(1,1) + HK40*P(1,2) + HK41*P(1,3);
|
||||
// const float HK50 = HK4/(HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
|
||||
Vector2f innov_var;
|
||||
Vector24f H;
|
||||
sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
|
||||
innov_var.copyTo(_aid_src_optical_flow.innovation_variance);
|
||||
|
||||
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
|
||||
_aid_src_optical_flow.innovation_variance[0] = (HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
|
||||
|
||||
if (_aid_src_optical_flow.innovation_variance[0] < R_LOS) {
|
||||
if ((_aid_src_optical_flow.innovation_variance[0] < R_LOS)
|
||||
|| (_aid_src_optical_flow.innovation_variance[1] < R_LOS)) {
|
||||
// we need to reinitialise the covariance matrix and abort this fusion step
|
||||
initialiseCovariance();
|
||||
return;
|
||||
}
|
||||
const float HK50 = HK4 / _aid_src_optical_flow.innovation_variance[0];
|
||||
|
||||
const float HK51 = Tbs(0,1)*q1;
|
||||
const float HK52 = Tbs(0,2)*q0;
|
||||
const float HK53 = Tbs(0,0)*q2;
|
||||
const float HK54 = HK51 + HK52 - HK53;
|
||||
const float HK55 = Tbs(0,0)*q3;
|
||||
const float HK56 = Tbs(0,1)*q0;
|
||||
const float HK57 = Tbs(0,2)*q1;
|
||||
const float HK58 = HK55 + HK56 - HK57;
|
||||
const float HK59 = Tbs(0,0)*q0;
|
||||
const float HK60 = Tbs(0,2)*q2;
|
||||
const float HK61 = Tbs(0,1)*q3;
|
||||
const float HK62 = HK59 + HK60 - HK61;
|
||||
const float HK63 = HK54*vd + HK58*ve + HK62*vn;
|
||||
const float HK64 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
|
||||
const float HK65 = HK58*vd + HK64*vn;
|
||||
const float HK66 = -HK54*ve + HK65;
|
||||
const float HK67 = HK54*vn + HK64*ve;
|
||||
const float HK68 = -HK62*vd + HK67;
|
||||
const float HK69 = HK62*ve + HK64*vd;
|
||||
const float HK70 = -HK58*vn + HK69;
|
||||
const float HK71 = 2*Tbs(0,1);
|
||||
const float HK72 = 2*Tbs(0,2);
|
||||
const float HK73 = HK12*HK72 + HK24*Tbs(0,0);
|
||||
const float HK74 = -HK16*HK71 + HK73;
|
||||
const float HK75 = 2*Tbs(0,0);
|
||||
const float HK76 = HK26*HK75 + HK32*Tbs(0,1);
|
||||
const float HK77 = -HK30*HK72 + HK76;
|
||||
const float HK78 = HK34*HK71 + HK36*Tbs(0,2);
|
||||
const float HK79 = -HK35*HK75 + HK78;
|
||||
const float HK80 = 2*HK63;
|
||||
const float HK81 = 2*HK65 + 2*ve*(-HK51 - HK52 + HK53);
|
||||
const float HK82 = 2*HK67 + 2*vd*(-HK59 - HK60 + HK61);
|
||||
const float HK83 = 2*HK69 + 2*vn*(-HK55 - HK56 + HK57);
|
||||
const float HK84 = HK71*(-HK14 + HK15) + HK73;
|
||||
const float HK85 = HK72*(-HK28 + HK29) + HK76;
|
||||
const float HK86 = HK75*(-HK10 + HK11) + HK78;
|
||||
const float HK87 = HK80*P(0,0) + HK81*P(0,1) + HK82*P(0,2) + HK83*P(0,3) + HK84*P(0,4) + HK85*P(0,5) + HK86*P(0,6);
|
||||
const float HK88 = HK80*P(0,6) + HK81*P(1,6) + HK82*P(2,6) + HK83*P(3,6) + HK84*P(4,6) + HK85*P(5,6) + HK86*P(6,6);
|
||||
const float HK89 = HK80*P(0,5) + HK81*P(1,5) + HK82*P(2,5) + HK83*P(3,5) + HK84*P(4,5) + HK85*P(5,5) + HK86*P(5,6);
|
||||
const float HK90 = HK80*P(0,4) + HK81*P(1,4) + HK82*P(2,4) + HK83*P(3,4) + HK84*P(4,4) + HK85*P(4,5) + HK86*P(4,6);
|
||||
const float HK91 = HK80*P(0,2) + HK81*P(1,2) + HK82*P(2,2) + HK83*P(2,3) + HK84*P(2,4) + HK85*P(2,5) + HK86*P(2,6);
|
||||
const float HK92 = 2*HK43;
|
||||
const float HK93 = HK80*P(0,3) + HK81*P(1,3) + HK82*P(2,3) + HK83*P(3,3) + HK84*P(3,4) + HK85*P(3,5) + HK86*P(3,6);
|
||||
const float HK94 = HK80*P(0,1) + HK81*P(1,1) + HK82*P(1,2) + HK83*P(1,3) + HK84*P(1,4) + HK85*P(1,5) + HK86*P(1,6);
|
||||
// const float HK95 = HK4/(HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
|
||||
|
||||
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
|
||||
_aid_src_optical_flow.innovation_variance[1] = (HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
|
||||
if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) {
|
||||
// we need to reinitialise the covariance matrix and abort this fusion step
|
||||
initialiseCovariance();
|
||||
return;
|
||||
}
|
||||
|
||||
const float HK95 = HK4 / _aid_src_optical_flow.innovation_variance[1];
|
||||
|
||||
// run the innovation consistency check and record result
|
||||
setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f));
|
||||
@@ -267,28 +148,10 @@ void Ekf::fuseOptFlow()
|
||||
// fuse observation axes sequentially
|
||||
{
|
||||
// Optical flow observation Jacobians - axis 0
|
||||
SparseVector24f<0,1,2,3,4,5,6> Hfusion;
|
||||
Hfusion.at<0>() = HK3*HK5;
|
||||
Hfusion.at<1>() = HK5*HK7;
|
||||
Hfusion.at<2>() = HK5*HK8;
|
||||
Hfusion.at<3>() = HK5*HK9;
|
||||
Hfusion.at<4>() = HK25*HK4;
|
||||
Hfusion.at<5>() = HK33*HK4;
|
||||
Hfusion.at<6>() = HK37*HK4;
|
||||
SparseVector24f<0,1,2,3,4,5,6> Hfusion(H);
|
||||
|
||||
// Optical flow Kalman gains - axis 0
|
||||
Vector24f Kfusion;
|
||||
Kfusion(0) = HK42*HK50;
|
||||
Kfusion(1) = HK49*HK50;
|
||||
Kfusion(2) = HK47*HK50;
|
||||
Kfusion(3) = HK48*HK50;
|
||||
Kfusion(4) = HK46*HK50;
|
||||
Kfusion(5) = HK45*HK50;
|
||||
Kfusion(6) = HK44*HK50;
|
||||
|
||||
for (unsigned row = 7; row <= 23; row++) {
|
||||
Kfusion(row) = HK50*(HK25*P(4,row) + HK33*P(5,row) + HK37*P(6,row) + HK38*P(0,row) + HK39*P(1,row) + HK40*P(2,row) + HK41*P(3,row));
|
||||
}
|
||||
Vector24f Kfusion = P * Hfusion / _aid_src_optical_flow.innovation_variance[0];
|
||||
|
||||
if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[0])) {
|
||||
fused[0] = true;
|
||||
@@ -301,29 +164,12 @@ void Ekf::fuseOptFlow()
|
||||
}
|
||||
|
||||
{
|
||||
sym::ComputeFlowYInnovVarAndH(state_vector, P, range, R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H);
|
||||
// Optical flow observation Jacobians - axis 1
|
||||
SparseVector24f<0,1,2,3,4,5,6> Hfusion;
|
||||
Hfusion.at<0>() = -HK5*HK63;
|
||||
Hfusion.at<1>() = -HK5*HK66;
|
||||
Hfusion.at<2>() = -HK5*HK68;
|
||||
Hfusion.at<3>() = -HK5*HK70;
|
||||
Hfusion.at<4>() = -HK4*HK74;
|
||||
Hfusion.at<5>() = -HK4*HK77;
|
||||
Hfusion.at<6>() = -HK4*HK79;
|
||||
SparseVector24f<0,1,2,3,4,5,6> Hfusion(H);
|
||||
|
||||
// Optical flow Kalman gains - axis 1
|
||||
Vector24f Kfusion;
|
||||
Kfusion(0) = -HK87*HK95;
|
||||
Kfusion(1) = -HK94*HK95;
|
||||
Kfusion(2) = -HK91*HK95;
|
||||
Kfusion(3) = -HK93*HK95;
|
||||
Kfusion(4) = -HK90*HK95;
|
||||
Kfusion(5) = -HK89*HK95;
|
||||
Kfusion(6) = -HK88*HK95;
|
||||
|
||||
for (unsigned row = 7; row <= 23; row++) {
|
||||
Kfusion(row) = -HK95*(HK80*P(0,row) + HK81*P(1,row) + HK82*P(2,row) + HK83*P(3,row) + HK84*P(4,row) + HK85*P(5,row) + HK86*P(6,row));
|
||||
}
|
||||
Vector24f Kfusion = P * Hfusion / _aid_src_optical_flow.innovation_variance[1];
|
||||
|
||||
if (measurementUpdate(Kfusion, Hfusion, _aid_src_optical_flow.innovation[1])) {
|
||||
fused[1] = true;
|
||||
|
||||
@@ -337,6 +337,56 @@ def compute_mag_declination_innov_innov_var_and_h(
|
||||
|
||||
return (innov, innov_var, H.T)
|
||||
|
||||
def predict_opt_flow(state, distance, epsilon):
|
||||
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
|
||||
|
||||
# Calculate earth relative velocity in a non-rotating sensor frame
|
||||
v = sf.V3(state[State.vx], state[State.vy], state[State.vz])
|
||||
rel_vel_sensor = R_to_body * v
|
||||
|
||||
# Divide by range to get predicted angular LOS rates relative to X and Y
|
||||
# axes. Note these are rates in a non-rotating sensor frame
|
||||
flow_pred = sf.V2()
|
||||
flow_pred[0] = rel_vel_sensor[1] / distance
|
||||
flow_pred[1] = -rel_vel_sensor[0] / distance
|
||||
flow_pred = add_epsilon_sign(flow_pred, distance, epsilon)
|
||||
|
||||
return flow_pred
|
||||
|
||||
|
||||
def compute_flow_xy_innov_var_and_hx(
|
||||
state: VState,
|
||||
P: MState,
|
||||
distance: sf.Scalar,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.V2, VState):
|
||||
meas_pred = predict_opt_flow(state, distance, epsilon);
|
||||
|
||||
innov_var = sf.V2()
|
||||
Hx = sf.V1(meas_pred[0]).jacobian(state)
|
||||
innov_var[0] = (Hx * P * Hx.T + R)[0,0]
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state)
|
||||
innov_var[1] = (Hy * P * Hy.T + R)[0,0]
|
||||
|
||||
return (innov_var, Hx.T)
|
||||
|
||||
def compute_flow_y_innov_var_and_h(
|
||||
state: VState,
|
||||
P: MState,
|
||||
distance: sf.Scalar,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VState):
|
||||
meas_pred = predict_opt_flow(state, distance, epsilon);
|
||||
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state)
|
||||
innov_var = (Hy * P * Hy.T + R)[0,0]
|
||||
|
||||
return (innov_var, Hy.T)
|
||||
|
||||
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"])
|
||||
@@ -352,3 +402,5 @@ generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=["
|
||||
generate_px4_function(compute_yaw_312_innov_var_and_h, output_names=["innov_var", "H"])
|
||||
generate_px4_function(compute_yaw_312_innov_var_and_h_alternate, output_names=["innov_var", "H"])
|
||||
generate_px4_function(compute_mag_declination_innov_innov_var_and_h, output_names=["innov", "innov_var", "H"])
|
||||
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"])
|
||||
|
||||
+127
@@ -0,0 +1,127 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// 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_flow_xy_innov_var_and_hx
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* distance: Scalar
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Matrix21
|
||||
* H: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar distance,
|
||||
const Scalar R, const Scalar epsilon,
|
||||
matrix::Matrix<Scalar, 2, 1>* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 285
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (29)
|
||||
const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp3 =
|
||||
Scalar(1.0) /
|
||||
(distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
|
||||
const Scalar _tmp4 = _tmp3 * (_tmp0 - _tmp1 + _tmp2);
|
||||
const Scalar _tmp5 = 2 * state(3, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(0, 0);
|
||||
const Scalar _tmp7 = 2 * state(2, 0);
|
||||
const Scalar _tmp8 = _tmp7 * state(1, 0);
|
||||
const Scalar _tmp9 = _tmp3 * (-_tmp6 + _tmp8);
|
||||
const Scalar _tmp10 = 2 * state(4, 0);
|
||||
const Scalar _tmp11 = _tmp10 * state(0, 0);
|
||||
const Scalar _tmp12 = 2 * state(5, 0);
|
||||
const Scalar _tmp13 = _tmp12 * state(3, 0);
|
||||
const Scalar _tmp14 = _tmp7 * state(6, 0);
|
||||
const Scalar _tmp15 = _tmp3 * (-_tmp11 - _tmp13 + _tmp14);
|
||||
const Scalar _tmp16 = 2 * state(1, 0);
|
||||
const Scalar _tmp17 =
|
||||
_tmp3 * (-_tmp10 * state(3, 0) + _tmp12 * state(0, 0) + _tmp16 * state(6, 0));
|
||||
const Scalar _tmp18 = _tmp7 * state(4, 0);
|
||||
const Scalar _tmp19 = _tmp12 * state(1, 0);
|
||||
const Scalar _tmp20 = 2 * state(0, 0) * state(6, 0);
|
||||
const Scalar _tmp21 = _tmp3 * (_tmp18 - _tmp19 + _tmp20);
|
||||
const Scalar _tmp22 = _tmp3 * (_tmp10 * state(1, 0) + _tmp5 * state(6, 0) + _tmp7 * state(5, 0));
|
||||
const Scalar _tmp23 = _tmp3 * (_tmp16 * state(0, 0) + _tmp7 * state(3, 0));
|
||||
const Scalar _tmp24 = _tmp3 * (-_tmp0 + _tmp1 + _tmp2);
|
||||
const Scalar _tmp25 = _tmp3 * (_tmp6 + _tmp8);
|
||||
const Scalar _tmp26 = _tmp3 * (_tmp16 * state(3, 0) - _tmp7 * state(0, 0));
|
||||
const Scalar _tmp27 = _tmp3 * (_tmp11 + _tmp13 - _tmp14);
|
||||
const Scalar _tmp28 = _tmp3 * (-_tmp18 + _tmp19 - _tmp20);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
matrix::Matrix<Scalar, 2, 1>& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var(0, 0) =
|
||||
R +
|
||||
_tmp15 * (P(0, 3) * _tmp17 + P(1, 3) * _tmp21 + P(2, 3) * _tmp22 + P(3, 3) * _tmp15 +
|
||||
P(4, 3) * _tmp9 + P(5, 3) * _tmp4 + P(6, 3) * _tmp23) +
|
||||
_tmp17 * (P(0, 0) * _tmp17 + P(1, 0) * _tmp21 + P(2, 0) * _tmp22 + P(3, 0) * _tmp15 +
|
||||
P(4, 0) * _tmp9 + P(5, 0) * _tmp4 + P(6, 0) * _tmp23) +
|
||||
_tmp21 * (P(0, 1) * _tmp17 + P(1, 1) * _tmp21 + P(2, 1) * _tmp22 + P(3, 1) * _tmp15 +
|
||||
P(4, 1) * _tmp9 + P(5, 1) * _tmp4 + P(6, 1) * _tmp23) +
|
||||
_tmp22 * (P(0, 2) * _tmp17 + P(1, 2) * _tmp21 + P(2, 2) * _tmp22 + P(3, 2) * _tmp15 +
|
||||
P(4, 2) * _tmp9 + P(5, 2) * _tmp4 + P(6, 2) * _tmp23) +
|
||||
_tmp23 * (P(0, 6) * _tmp17 + P(1, 6) * _tmp21 + P(2, 6) * _tmp22 + P(3, 6) * _tmp15 +
|
||||
P(4, 6) * _tmp9 + P(5, 6) * _tmp4 + P(6, 6) * _tmp23) +
|
||||
_tmp4 * (P(0, 5) * _tmp17 + P(1, 5) * _tmp21 + P(2, 5) * _tmp22 + P(3, 5) * _tmp15 +
|
||||
P(4, 5) * _tmp9 + P(5, 5) * _tmp4 + P(6, 5) * _tmp23) +
|
||||
_tmp9 * (P(0, 4) * _tmp17 + P(1, 4) * _tmp21 + P(2, 4) * _tmp22 + P(3, 4) * _tmp15 +
|
||||
P(4, 4) * _tmp9 + P(5, 4) * _tmp4 + P(6, 4) * _tmp23);
|
||||
_innov_var(1, 0) =
|
||||
R -
|
||||
_tmp17 * (-P(0, 3) * _tmp27 - P(1, 3) * _tmp22 - P(2, 3) * _tmp28 - P(3, 3) * _tmp17 -
|
||||
P(4, 3) * _tmp24 - P(5, 3) * _tmp25 - P(6, 3) * _tmp26) -
|
||||
_tmp22 * (-P(0, 1) * _tmp27 - P(1, 1) * _tmp22 - P(2, 1) * _tmp28 - P(3, 1) * _tmp17 -
|
||||
P(4, 1) * _tmp24 - P(5, 1) * _tmp25 - P(6, 1) * _tmp26) -
|
||||
_tmp24 * (-P(0, 4) * _tmp27 - P(1, 4) * _tmp22 - P(2, 4) * _tmp28 - P(3, 4) * _tmp17 -
|
||||
P(4, 4) * _tmp24 - P(5, 4) * _tmp25 - P(6, 4) * _tmp26) -
|
||||
_tmp25 * (-P(0, 5) * _tmp27 - P(1, 5) * _tmp22 - P(2, 5) * _tmp28 - P(3, 5) * _tmp17 -
|
||||
P(4, 5) * _tmp24 - P(5, 5) * _tmp25 - P(6, 5) * _tmp26) -
|
||||
_tmp26 * (-P(0, 6) * _tmp27 - P(1, 6) * _tmp22 - P(2, 6) * _tmp28 - P(3, 6) * _tmp17 -
|
||||
P(4, 6) * _tmp24 - P(5, 6) * _tmp25 - P(6, 6) * _tmp26) -
|
||||
_tmp27 * (-P(0, 0) * _tmp27 - P(1, 0) * _tmp22 - P(2, 0) * _tmp28 - P(3, 0) * _tmp17 -
|
||||
P(4, 0) * _tmp24 - P(5, 0) * _tmp25 - P(6, 0) * _tmp26) -
|
||||
_tmp28 * (-P(0, 2) * _tmp27 - P(1, 2) * _tmp22 - P(2, 2) * _tmp28 - P(3, 2) * _tmp17 -
|
||||
P(4, 2) * _tmp24 - P(5, 2) * _tmp25 - P(6, 2) * _tmp26);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
|
||||
_H.setZero();
|
||||
|
||||
_H(0, 0) = _tmp17;
|
||||
_H(1, 0) = _tmp21;
|
||||
_H(2, 0) = _tmp22;
|
||||
_H(3, 0) = _tmp15;
|
||||
_H(4, 0) = _tmp9;
|
||||
_H(5, 0) = _tmp4;
|
||||
_H(6, 0) = _tmp23;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
@@ -0,0 +1,95 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// 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_flow_y_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* distance: Scalar
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar distance,
|
||||
const Scalar R, const Scalar epsilon,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 171
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (13)
|
||||
const Scalar _tmp0 =
|
||||
Scalar(1.0) /
|
||||
(distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
|
||||
const Scalar _tmp1 =
|
||||
_tmp0 * (std::pow(state(0, 0), Scalar(2)) + std::pow(state(1, 0), Scalar(2)) -
|
||||
std::pow(state(2, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)));
|
||||
const Scalar _tmp2 = 2 * state(0, 0);
|
||||
const Scalar _tmp3 = 2 * state(1, 0);
|
||||
const Scalar _tmp4 = _tmp0 * (_tmp2 * state(3, 0) + _tmp3 * state(2, 0));
|
||||
const Scalar _tmp5 = _tmp0 * (-_tmp2 * state(2, 0) + _tmp3 * state(3, 0));
|
||||
const Scalar _tmp6 = 2 * state(4, 0);
|
||||
const Scalar _tmp7 = 2 * state(6, 0);
|
||||
const Scalar _tmp8 = _tmp0 * (_tmp2 * state(5, 0) - _tmp6 * state(3, 0) + _tmp7 * state(1, 0));
|
||||
const Scalar _tmp9 = 2 * state(5, 0);
|
||||
const Scalar _tmp10 = _tmp0 * (_tmp2 * state(4, 0) - _tmp7 * state(2, 0) + _tmp9 * state(3, 0));
|
||||
const Scalar _tmp11 = _tmp0 * (_tmp3 * state(4, 0) + _tmp7 * state(3, 0) + _tmp9 * state(2, 0));
|
||||
const Scalar _tmp12 = _tmp0 * (_tmp3 * state(5, 0) - _tmp6 * state(2, 0) - _tmp7 * state(0, 0));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R -
|
||||
_tmp1 * (-P(0, 4) * _tmp10 - P(1, 4) * _tmp11 - P(2, 4) * _tmp12 -
|
||||
P(3, 4) * _tmp8 - P(4, 4) * _tmp1 - P(5, 4) * _tmp4 - P(6, 4) * _tmp5) -
|
||||
_tmp10 * (-P(0, 0) * _tmp10 - P(1, 0) * _tmp11 - P(2, 0) * _tmp12 -
|
||||
P(3, 0) * _tmp8 - P(4, 0) * _tmp1 - P(5, 0) * _tmp4 - P(6, 0) * _tmp5) -
|
||||
_tmp11 * (-P(0, 1) * _tmp10 - P(1, 1) * _tmp11 - P(2, 1) * _tmp12 -
|
||||
P(3, 1) * _tmp8 - P(4, 1) * _tmp1 - P(5, 1) * _tmp4 - P(6, 1) * _tmp5) -
|
||||
_tmp12 * (-P(0, 2) * _tmp10 - P(1, 2) * _tmp11 - P(2, 2) * _tmp12 -
|
||||
P(3, 2) * _tmp8 - P(4, 2) * _tmp1 - P(5, 2) * _tmp4 - P(6, 2) * _tmp5) -
|
||||
_tmp4 * (-P(0, 5) * _tmp10 - P(1, 5) * _tmp11 - P(2, 5) * _tmp12 -
|
||||
P(3, 5) * _tmp8 - P(4, 5) * _tmp1 - P(5, 5) * _tmp4 - P(6, 5) * _tmp5) -
|
||||
_tmp5 * (-P(0, 6) * _tmp10 - P(1, 6) * _tmp11 - P(2, 6) * _tmp12 -
|
||||
P(3, 6) * _tmp8 - P(4, 6) * _tmp1 - P(5, 6) * _tmp4 - P(6, 6) * _tmp5) -
|
||||
_tmp8 * (-P(0, 3) * _tmp10 - P(1, 3) * _tmp11 - P(2, 3) * _tmp12 -
|
||||
P(3, 3) * _tmp8 - P(4, 3) * _tmp1 - P(5, 3) * _tmp4 - P(6, 3) * _tmp5);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
|
||||
_H.setZero();
|
||||
|
||||
_H(0, 0) = -_tmp10;
|
||||
_H(1, 0) = -_tmp11;
|
||||
_H(2, 0) = -_tmp12;
|
||||
_H(3, 0) = -_tmp8;
|
||||
_H(4, 0) = -_tmp1;
|
||||
_H(5, 0) = -_tmp4;
|
||||
_H(6, 0) = -_tmp5;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
Reference in New Issue
Block a user