mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-23 03:43:33 +08:00
ekf2-gravity: nomalize gravity fusion and proper sequential fusion
This commit is contained in:
bresch
@@ -607,35 +607,64 @@ def compute_drag_y_innov_var_and_k(
|
||||
|
||||
return (innov_var, K)
|
||||
|
||||
def compute_gravity_innov_var_and_k_and_h(
|
||||
state: VState,
|
||||
P: MTangent,
|
||||
meas: sf.V3,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.V3, sf.V3, VTangent, VTangent, VTangent):
|
||||
|
||||
state = vstate_to_state(state)
|
||||
def predict_gravity_direction(state: State):
|
||||
# get transform from earth to body frame
|
||||
R_to_body = state["quat_nominal"].inverse()
|
||||
|
||||
# the innovation is the error between measured acceleration
|
||||
# and predicted (body frame), assuming no body acceleration
|
||||
meas_pred = R_to_body * sf.Matrix([0,0,-9.80665])
|
||||
innov = meas_pred - 9.80665 * meas.normalized(epsilon=epsilon)
|
||||
return R_to_body * sf.Matrix([0,0,-1])
|
||||
|
||||
def compute_gravity_xyz_innov_var_and_hx(
|
||||
state: VState,
|
||||
P: MTangent,
|
||||
R: sf.Scalar
|
||||
) -> (sf.V3, VTangent):
|
||||
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_gravity_direction(state)
|
||||
|
||||
# initialize outputs
|
||||
innov_var = sf.V3()
|
||||
K = [None] * 3
|
||||
H = [None] * 3
|
||||
|
||||
# calculate observation jacobian (H), kalman gain (K), and innovation variance (S)
|
||||
# for each axis
|
||||
for i in range(3):
|
||||
H = sf.V1(meas_pred[i]).jacobian(state)
|
||||
innov_var[i] = (H * P * H.T + R)[0,0]
|
||||
K[i] = P * H.T / innov_var[i]
|
||||
H[i] = sf.V1(meas_pred[i]).jacobian(state)
|
||||
innov_var[i] = (H[i] * P * H[i].T + R)[0,0]
|
||||
|
||||
return (innov, innov_var, K[0], K[1], K[2])
|
||||
return (innov_var, H[0].T)
|
||||
|
||||
def compute_gravity_y_innov_var_and_h(
|
||||
state: VState,
|
||||
P: MTangent,
|
||||
R: sf.Scalar
|
||||
) -> (sf.V3, VTangent, VTangent, VTangent):
|
||||
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_gravity_direction(state)
|
||||
|
||||
# calculate observation jacobian (H), kalman gain (K), and innovation variance (S)
|
||||
H = sf.V1(meas_pred[1]).jacobian(state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
|
||||
def compute_gravity_z_innov_var_and_h(
|
||||
state: VState,
|
||||
P: MTangent,
|
||||
R: sf.Scalar
|
||||
) -> (sf.V3, VTangent, VTangent, VTangent):
|
||||
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_gravity_direction(state)
|
||||
|
||||
# calculate observation jacobian (H), kalman gain (K), and innovation variance (S)
|
||||
H = sf.V1(meas_pred[2]).jacobian(state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
|
||||
print("Derive EKF2 equations...")
|
||||
generate_px4_function(predict_covariance, output_names=None)
|
||||
@@ -662,6 +691,8 @@ generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=["
|
||||
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_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "H"])
|
||||
generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"])
|
||||
generate_px4_function(compute_gravity_xyz_innov_var_and_hx, output_names=["innov_var", "Hx"])
|
||||
generate_px4_function(compute_gravity_y_innov_var_and_h, output_names=["innov_var", "Hy"])
|
||||
generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_var", "Hz"])
|
||||
|
||||
generate_px4_state(State, tangent_idx)
|
||||
|
||||
-191
@@ -1,191 +0,0 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// This file was autogenerated by symforce from template:
|
||||
// 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_gravity_innov_var_and_k_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* meas: Matrix31
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov: Matrix31
|
||||
* innov_var: Matrix31
|
||||
* Kx: Matrix23_1
|
||||
* Ky: Matrix23_1
|
||||
* Kz: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P,
|
||||
const matrix::Matrix<Scalar, 3, 1>& meas, const Scalar R,
|
||||
const Scalar epsilon,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov = nullptr,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Kx = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Ky = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Kz = nullptr) {
|
||||
// Total ops: 361
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (31)
|
||||
const Scalar _tmp0 = 2 * state(2, 0);
|
||||
const Scalar _tmp1 = _tmp0 * state(0, 0);
|
||||
const Scalar _tmp2 = 2 * state(1, 0);
|
||||
const Scalar _tmp3 = _tmp2 * state(3, 0);
|
||||
const Scalar _tmp4 = -_tmp1 + _tmp3;
|
||||
const Scalar _tmp5 =
|
||||
Scalar(9.8066499999999994) /
|
||||
std::sqrt(Scalar(epsilon + std::pow(meas(0, 0), Scalar(2)) + std::pow(meas(1, 0), Scalar(2)) +
|
||||
std::pow(meas(2, 0), Scalar(2))));
|
||||
const Scalar _tmp6 = _tmp0 * state(3, 0);
|
||||
const Scalar _tmp7 = _tmp2 * state(0, 0);
|
||||
const Scalar _tmp8 = _tmp6 + _tmp7;
|
||||
const Scalar _tmp9 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp10 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp11 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp12 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp13 = -Scalar(9.8066499999999994) * _tmp10 + Scalar(9.8066499999999994) * _tmp11 +
|
||||
Scalar(9.8066499999999994) * _tmp12 - Scalar(9.8066499999999994) * _tmp9;
|
||||
const Scalar _tmp14 = -Scalar(9.8066499999999994) * _tmp8;
|
||||
const Scalar _tmp15 = P(2, 2) * _tmp14;
|
||||
const Scalar _tmp16 = P(1, 1) * _tmp13;
|
||||
const Scalar _tmp17 =
|
||||
R + _tmp13 * (P(2, 1) * _tmp14 + _tmp16) + _tmp14 * (P(1, 2) * _tmp13 + _tmp15);
|
||||
const Scalar _tmp18 = Scalar(9.8066499999999994) * _tmp10 - Scalar(9.8066499999999994) * _tmp11 -
|
||||
Scalar(9.8066499999999994) * _tmp12 + Scalar(9.8066499999999994) * _tmp9;
|
||||
const Scalar _tmp19 = -Scalar(9.8066499999999994) * _tmp1 + Scalar(9.8066499999999994) * _tmp3;
|
||||
const Scalar _tmp20 = P(2, 2) * _tmp19;
|
||||
const Scalar _tmp21 = P(0, 0) * _tmp18;
|
||||
const Scalar _tmp22 =
|
||||
R + _tmp18 * (P(2, 0) * _tmp19 + _tmp21) + _tmp19 * (P(0, 2) * _tmp18 + _tmp20);
|
||||
const Scalar _tmp23 = Scalar(9.8066499999999994) * _tmp6 + Scalar(9.8066499999999994) * _tmp7;
|
||||
const Scalar _tmp24 = -Scalar(9.8066499999999994) * _tmp4;
|
||||
const Scalar _tmp25 = P(1, 1) * _tmp24;
|
||||
const Scalar _tmp26 = P(0, 0) * _tmp23;
|
||||
const Scalar _tmp27 =
|
||||
R + _tmp23 * (P(1, 0) * _tmp24 + _tmp26) + _tmp24 * (P(0, 1) * _tmp23 + _tmp25);
|
||||
const Scalar _tmp28 = Scalar(1.0) / (_tmp17);
|
||||
const Scalar _tmp29 = Scalar(1.0) / (_tmp22);
|
||||
const Scalar _tmp30 = Scalar(1.0) / (_tmp27);
|
||||
|
||||
// Output terms (5)
|
||||
if (innov != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _innov = (*innov);
|
||||
|
||||
_innov(0, 0) = -Scalar(9.8066499999999994) * _tmp4 - _tmp5 * meas(0, 0);
|
||||
_innov(1, 0) = -_tmp5 * meas(1, 0) - Scalar(9.8066499999999994) * _tmp8;
|
||||
_innov(2, 0) = Scalar(19.613299999999999) * _tmp10 - _tmp5 * meas(2, 0) +
|
||||
Scalar(19.613299999999999) * _tmp9 + Scalar(-9.8066499999999994);
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var(0, 0) = _tmp17;
|
||||
_innov_var(1, 0) = _tmp22;
|
||||
_innov_var(2, 0) = _tmp27;
|
||||
}
|
||||
|
||||
if (Kx != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _kx = (*Kx);
|
||||
|
||||
_kx(0, 0) = _tmp28 * (P(0, 1) * _tmp13 + P(0, 2) * _tmp14);
|
||||
_kx(1, 0) = _tmp28 * (P(1, 2) * _tmp14 + _tmp16);
|
||||
_kx(2, 0) = _tmp28 * (P(2, 1) * _tmp13 + _tmp15);
|
||||
_kx(3, 0) = _tmp28 * (P(3, 1) * _tmp13 + P(3, 2) * _tmp14);
|
||||
_kx(4, 0) = _tmp28 * (P(4, 1) * _tmp13 + P(4, 2) * _tmp14);
|
||||
_kx(5, 0) = _tmp28 * (P(5, 1) * _tmp13 + P(5, 2) * _tmp14);
|
||||
_kx(6, 0) = _tmp28 * (P(6, 1) * _tmp13 + P(6, 2) * _tmp14);
|
||||
_kx(7, 0) = _tmp28 * (P(7, 1) * _tmp13 + P(7, 2) * _tmp14);
|
||||
_kx(8, 0) = _tmp28 * (P(8, 1) * _tmp13 + P(8, 2) * _tmp14);
|
||||
_kx(9, 0) = _tmp28 * (P(9, 1) * _tmp13 + P(9, 2) * _tmp14);
|
||||
_kx(10, 0) = _tmp28 * (P(10, 1) * _tmp13 + P(10, 2) * _tmp14);
|
||||
_kx(11, 0) = _tmp28 * (P(11, 1) * _tmp13 + P(11, 2) * _tmp14);
|
||||
_kx(12, 0) = _tmp28 * (P(12, 1) * _tmp13 + P(12, 2) * _tmp14);
|
||||
_kx(13, 0) = _tmp28 * (P(13, 1) * _tmp13 + P(13, 2) * _tmp14);
|
||||
_kx(14, 0) = _tmp28 * (P(14, 1) * _tmp13 + P(14, 2) * _tmp14);
|
||||
_kx(15, 0) = _tmp28 * (P(15, 1) * _tmp13 + P(15, 2) * _tmp14);
|
||||
_kx(16, 0) = _tmp28 * (P(16, 1) * _tmp13 + P(16, 2) * _tmp14);
|
||||
_kx(17, 0) = _tmp28 * (P(17, 1) * _tmp13 + P(17, 2) * _tmp14);
|
||||
_kx(18, 0) = _tmp28 * (P(18, 1) * _tmp13 + P(18, 2) * _tmp14);
|
||||
_kx(19, 0) = _tmp28 * (P(19, 1) * _tmp13 + P(19, 2) * _tmp14);
|
||||
_kx(20, 0) = _tmp28 * (P(20, 1) * _tmp13 + P(20, 2) * _tmp14);
|
||||
_kx(21, 0) = _tmp28 * (P(21, 1) * _tmp13 + P(21, 2) * _tmp14);
|
||||
_kx(22, 0) = _tmp28 * (P(22, 1) * _tmp13 + P(22, 2) * _tmp14);
|
||||
}
|
||||
|
||||
if (Ky != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _ky = (*Ky);
|
||||
|
||||
_ky(0, 0) = _tmp29 * (P(0, 2) * _tmp19 + _tmp21);
|
||||
_ky(1, 0) = _tmp29 * (P(1, 0) * _tmp18 + P(1, 2) * _tmp19);
|
||||
_ky(2, 0) = _tmp29 * (P(2, 0) * _tmp18 + _tmp20);
|
||||
_ky(3, 0) = _tmp29 * (P(3, 0) * _tmp18 + P(3, 2) * _tmp19);
|
||||
_ky(4, 0) = _tmp29 * (P(4, 0) * _tmp18 + P(4, 2) * _tmp19);
|
||||
_ky(5, 0) = _tmp29 * (P(5, 0) * _tmp18 + P(5, 2) * _tmp19);
|
||||
_ky(6, 0) = _tmp29 * (P(6, 0) * _tmp18 + P(6, 2) * _tmp19);
|
||||
_ky(7, 0) = _tmp29 * (P(7, 0) * _tmp18 + P(7, 2) * _tmp19);
|
||||
_ky(8, 0) = _tmp29 * (P(8, 0) * _tmp18 + P(8, 2) * _tmp19);
|
||||
_ky(9, 0) = _tmp29 * (P(9, 0) * _tmp18 + P(9, 2) * _tmp19);
|
||||
_ky(10, 0) = _tmp29 * (P(10, 0) * _tmp18 + P(10, 2) * _tmp19);
|
||||
_ky(11, 0) = _tmp29 * (P(11, 0) * _tmp18 + P(11, 2) * _tmp19);
|
||||
_ky(12, 0) = _tmp29 * (P(12, 0) * _tmp18 + P(12, 2) * _tmp19);
|
||||
_ky(13, 0) = _tmp29 * (P(13, 0) * _tmp18 + P(13, 2) * _tmp19);
|
||||
_ky(14, 0) = _tmp29 * (P(14, 0) * _tmp18 + P(14, 2) * _tmp19);
|
||||
_ky(15, 0) = _tmp29 * (P(15, 0) * _tmp18 + P(15, 2) * _tmp19);
|
||||
_ky(16, 0) = _tmp29 * (P(16, 0) * _tmp18 + P(16, 2) * _tmp19);
|
||||
_ky(17, 0) = _tmp29 * (P(17, 0) * _tmp18 + P(17, 2) * _tmp19);
|
||||
_ky(18, 0) = _tmp29 * (P(18, 0) * _tmp18 + P(18, 2) * _tmp19);
|
||||
_ky(19, 0) = _tmp29 * (P(19, 0) * _tmp18 + P(19, 2) * _tmp19);
|
||||
_ky(20, 0) = _tmp29 * (P(20, 0) * _tmp18 + P(20, 2) * _tmp19);
|
||||
_ky(21, 0) = _tmp29 * (P(21, 0) * _tmp18 + P(21, 2) * _tmp19);
|
||||
_ky(22, 0) = _tmp29 * (P(22, 0) * _tmp18 + P(22, 2) * _tmp19);
|
||||
}
|
||||
|
||||
if (Kz != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _kz = (*Kz);
|
||||
|
||||
_kz(0, 0) = _tmp30 * (P(0, 1) * _tmp24 + _tmp26);
|
||||
_kz(1, 0) = _tmp30 * (P(1, 0) * _tmp23 + _tmp25);
|
||||
_kz(2, 0) = _tmp30 * (P(2, 0) * _tmp23 + P(2, 1) * _tmp24);
|
||||
_kz(3, 0) = _tmp30 * (P(3, 0) * _tmp23 + P(3, 1) * _tmp24);
|
||||
_kz(4, 0) = _tmp30 * (P(4, 0) * _tmp23 + P(4, 1) * _tmp24);
|
||||
_kz(5, 0) = _tmp30 * (P(5, 0) * _tmp23 + P(5, 1) * _tmp24);
|
||||
_kz(6, 0) = _tmp30 * (P(6, 0) * _tmp23 + P(6, 1) * _tmp24);
|
||||
_kz(7, 0) = _tmp30 * (P(7, 0) * _tmp23 + P(7, 1) * _tmp24);
|
||||
_kz(8, 0) = _tmp30 * (P(8, 0) * _tmp23 + P(8, 1) * _tmp24);
|
||||
_kz(9, 0) = _tmp30 * (P(9, 0) * _tmp23 + P(9, 1) * _tmp24);
|
||||
_kz(10, 0) = _tmp30 * (P(10, 0) * _tmp23 + P(10, 1) * _tmp24);
|
||||
_kz(11, 0) = _tmp30 * (P(11, 0) * _tmp23 + P(11, 1) * _tmp24);
|
||||
_kz(12, 0) = _tmp30 * (P(12, 0) * _tmp23 + P(12, 1) * _tmp24);
|
||||
_kz(13, 0) = _tmp30 * (P(13, 0) * _tmp23 + P(13, 1) * _tmp24);
|
||||
_kz(14, 0) = _tmp30 * (P(14, 0) * _tmp23 + P(14, 1) * _tmp24);
|
||||
_kz(15, 0) = _tmp30 * (P(15, 0) * _tmp23 + P(15, 1) * _tmp24);
|
||||
_kz(16, 0) = _tmp30 * (P(16, 0) * _tmp23 + P(16, 1) * _tmp24);
|
||||
_kz(17, 0) = _tmp30 * (P(17, 0) * _tmp23 + P(17, 1) * _tmp24);
|
||||
_kz(18, 0) = _tmp30 * (P(18, 0) * _tmp23 + P(18, 1) * _tmp24);
|
||||
_kz(19, 0) = _tmp30 * (P(19, 0) * _tmp23 + P(19, 1) * _tmp24);
|
||||
_kz(20, 0) = _tmp30 * (P(20, 0) * _tmp23 + P(20, 1) * _tmp24);
|
||||
_kz(21, 0) = _tmp30 * (P(21, 0) * _tmp23 + P(21, 1) * _tmp24);
|
||||
_kz(22, 0) = _tmp30 * (P(22, 0) * _tmp23 + P(22, 1) * _tmp24);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
+77
@@ -0,0 +1,77 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// This file was autogenerated by symforce from template:
|
||||
// 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_gravity_xyz_innov_var_and_hx
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* R: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Matrix31
|
||||
* Hx: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeGravityXyzInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Hx = nullptr) {
|
||||
// Total ops: 51
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (16)
|
||||
const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp2 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp3 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp4 = _tmp0 + _tmp1 - _tmp2 - _tmp3;
|
||||
const Scalar _tmp5 = 2 * state(2, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp7 = 2 * state(1, 0);
|
||||
const Scalar _tmp8 = _tmp7 * state(0, 0);
|
||||
const Scalar _tmp9 = -_tmp6 - _tmp8;
|
||||
const Scalar _tmp10 = -_tmp0 - _tmp1 + _tmp2 + _tmp3;
|
||||
const Scalar _tmp11 = _tmp5 * state(0, 0);
|
||||
const Scalar _tmp12 = _tmp7 * state(3, 0);
|
||||
const Scalar _tmp13 = -_tmp11 + _tmp12;
|
||||
const Scalar _tmp14 = _tmp6 + _tmp8;
|
||||
const Scalar _tmp15 = _tmp11 - _tmp12;
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var(0, 0) = R + _tmp4 * (P(1, 1) * _tmp4 + P(2, 1) * _tmp9) +
|
||||
_tmp9 * (P(1, 2) * _tmp4 + P(2, 2) * _tmp9);
|
||||
_innov_var(1, 0) = R + _tmp10 * (P(0, 0) * _tmp10 + P(2, 0) * _tmp13) +
|
||||
_tmp13 * (P(0, 2) * _tmp10 + P(2, 2) * _tmp13);
|
||||
_innov_var(2, 0) = R + _tmp14 * (P(0, 0) * _tmp14 + P(1, 0) * _tmp15) +
|
||||
_tmp15 * (P(0, 1) * _tmp14 + P(1, 1) * _tmp15);
|
||||
}
|
||||
|
||||
if (Hx != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _hx = (*Hx);
|
||||
|
||||
_hx.setZero();
|
||||
|
||||
_hx(1, 0) = _tmp4;
|
||||
_hx(2, 0) = _tmp9;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
+60
@@ -0,0 +1,60 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// This file was autogenerated by symforce from template:
|
||||
// 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_gravity_y_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* R: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* Hy: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeGravityYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Hy = nullptr) {
|
||||
// Total ops: 22
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (2)
|
||||
const Scalar _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 _tmp1 = -2 * state(0, 0) * state(2, 0) + 2 * state(1, 0) * state(3, 0);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R + _tmp0 * (P(0, 0) * _tmp0 + P(2, 0) * _tmp1) +
|
||||
_tmp1 * (P(0, 2) * _tmp0 + P(2, 2) * _tmp1);
|
||||
}
|
||||
|
||||
if (Hy != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _hy = (*Hy);
|
||||
|
||||
_hy.setZero();
|
||||
|
||||
_hy(0, 0) = _tmp0;
|
||||
_hy(2, 0) = _tmp1;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
+61
@@ -0,0 +1,61 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// This file was autogenerated by symforce from template:
|
||||
// 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_gravity_z_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* R: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* Hz: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeGravityZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Hz = nullptr) {
|
||||
// Total ops: 18
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (4)
|
||||
const Scalar _tmp0 = 2 * state(2, 0);
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 = _tmp0 * state(3, 0) + _tmp1 * state(0, 0);
|
||||
const Scalar _tmp3 = _tmp0 * state(0, 0) - _tmp1 * state(3, 0);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R + _tmp2 * (P(0, 0) * _tmp2 + P(1, 0) * _tmp3) +
|
||||
_tmp3 * (P(0, 1) * _tmp2 + P(1, 1) * _tmp3);
|
||||
}
|
||||
|
||||
if (Hz != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _hz = (*Hz);
|
||||
|
||||
_hz.setZero();
|
||||
|
||||
_hz(0, 0) = _tmp2;
|
||||
_hz(1, 0) = _tmp3;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
Reference in New Issue
Block a user