mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-28 04:20:35 +08:00
ekf2: symforce - use builtin Rot3 and Quaternion operations
This commit is contained in:
bresch
@@ -69,6 +69,12 @@ class VState(sf.Matrix):
|
||||
class MState(sf.Matrix):
|
||||
SHAPE = (State.n_states, State.n_states)
|
||||
|
||||
def state_to_quat(state: VState) -> sf.Quaternion:
|
||||
return sf.Quaternion(sf.V3(state[State.qx], state[State.qy], state[State.qz]), state[State.qw])
|
||||
|
||||
def state_to_rot3(state: VState) -> sf.Rot3:
|
||||
return sf.Rot3(state_to_quat(state))
|
||||
|
||||
def predict_covariance(
|
||||
state: VState,
|
||||
P: MState,
|
||||
@@ -86,17 +92,17 @@ def predict_covariance(
|
||||
d_ang_b = sf.V3(state[State.d_ang_bx], state[State.d_ang_by], state[State.d_ang_bz])
|
||||
d_ang_true = d_ang - d_ang_b
|
||||
|
||||
q = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
|
||||
R_to_earth = quat_to_rot_simplified(q)
|
||||
q = state_to_quat(state)
|
||||
R_to_earth = sf.Rot3(q)
|
||||
v = sf.V3(state[State.vx], state[State.vy], state[State.vz])
|
||||
p = sf.V3(state[State.px], state[State.py], state[State.pz])
|
||||
|
||||
q_new = quat_mult(q, sf.V4(1, 0.5 * d_ang_true[0], 0.5 * d_ang_true[1], 0.5 * d_ang_true[2]))
|
||||
q_new = q * sf.Quaternion(sf.V3(0.5 * d_ang_true[0], 0.5 * d_ang_true[1], 0.5 * d_ang_true[2]), 1)
|
||||
v_new = v + R_to_earth * d_vel_true + sf.V3(0 ,0 ,g) * dt
|
||||
p_new = p + v * dt
|
||||
|
||||
# Predicted state vector at time t + dt
|
||||
state_new = VState.block_matrix([[q_new], [v_new], [p_new], [sf.Matrix(state[State.d_ang_bx:State.n_states])]])
|
||||
state_new = VState.block_matrix([[sf.V4(q_new.w, q_new.x, q_new.y, q_new.z)], [v_new], [p_new], [sf.Matrix(state[State.d_ang_bx:State.n_states])]])
|
||||
|
||||
# State propagation jacobian
|
||||
A = state_new.jacobian(state)
|
||||
@@ -155,8 +161,7 @@ def predict_sideslip(
|
||||
) -> (sf.Scalar):
|
||||
|
||||
vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz])
|
||||
q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
|
||||
relative_wind_body = quat_to_rot(q_att).T * vel_rel
|
||||
relative_wind_body = state_to_rot3(state).inverse() * vel_rel
|
||||
|
||||
# Small angle approximation of side slip model
|
||||
# Protect division by zero using epsilon
|
||||
@@ -196,11 +201,10 @@ def compute_sideslip_h_and_k(
|
||||
return (H.T, K)
|
||||
|
||||
def predict_mag_body(state) -> sf.V3:
|
||||
q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
|
||||
mag_field_earth = sf.V3(state[State.ix], state[State.iy], state[State.iz])
|
||||
mag_bias_body = sf.V3(state[State.ibx], state[State.iby], state[State.ibz])
|
||||
|
||||
mag_body = quat_to_rot(q_att).T * mag_field_earth + mag_bias_body
|
||||
mag_body = state_to_rot3(state).inverse() * mag_field_earth + mag_bias_body
|
||||
return mag_body
|
||||
|
||||
def compute_mag_innov_innov_var_and_hx(
|
||||
@@ -260,8 +264,7 @@ def compute_yaw_321_innov_var_and_h(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VState):
|
||||
|
||||
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_earth = state_to_rot3(state).to_rotation_matrix()
|
||||
# Fix the singularity at pi/2 by inserting epsilon
|
||||
meas_pred = sf.atan2(R_to_earth[1,0], R_to_earth[0,0], epsilon=epsilon)
|
||||
|
||||
@@ -277,8 +280,7 @@ def compute_yaw_321_innov_var_and_h_alternate(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VState):
|
||||
|
||||
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_earth = state_to_rot3(state).to_rotation_matrix()
|
||||
# Alternate form that has a singularity at yaw 0 instead of pi/2
|
||||
meas_pred = sf.pi/2 - sf.atan2(R_to_earth[0,0], R_to_earth[1,0], epsilon=epsilon)
|
||||
|
||||
@@ -294,8 +296,7 @@ def compute_yaw_312_innov_var_and_h(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VState):
|
||||
|
||||
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_earth = state_to_rot3(state).to_rotation_matrix()
|
||||
# Alternate form to be used when close to pitch +-pi/2
|
||||
meas_pred = sf.atan2(-R_to_earth[0,1], R_to_earth[1,1], epsilon=epsilon)
|
||||
|
||||
@@ -311,8 +312,7 @@ def compute_yaw_312_innov_var_and_h_alternate(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VState):
|
||||
|
||||
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_earth = state_to_rot3(state).to_rotation_matrix()
|
||||
# Alternate form to be used when close to pitch +-pi/2
|
||||
meas_pred = sf.pi/2 - sf.atan2(-R_to_earth[1,1], R_to_earth[0,1], epsilon=epsilon)
|
||||
|
||||
@@ -336,9 +336,7 @@ def compute_mag_declination_pred_innov_var_and_h(
|
||||
return (meas_pred, 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
|
||||
R_to_body = state_to_rot3(state).inverse()
|
||||
|
||||
# Calculate earth relative velocity in a non-rotating sensor frame
|
||||
v = sf.V3(state[State.vx], state[State.vy], state[State.vz])
|
||||
@@ -393,8 +391,7 @@ def compute_gnss_yaw_pred_innov_var_and_h(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, sf.Scalar, VState):
|
||||
|
||||
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_earth = state_to_rot3(state)
|
||||
|
||||
# define antenna vector in body frame
|
||||
ant_vec_bf = sf.V3(sf.cos(antenna_yaw_offset), sf.sin(antenna_yaw_offset), 0)
|
||||
@@ -417,9 +414,7 @@ def predict_drag(
|
||||
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
|
||||
R_to_body = state_to_rot3(state).inverse()
|
||||
|
||||
vel_rel = sf.V3(state[State.vx] - state[State.wx],
|
||||
state[State.vy] - state[State.wy],
|
||||
@@ -481,8 +476,7 @@ def compute_gravity_innov_var_and_k_and_h(
|
||||
) -> (sf.V3, sf.V3, VState, VState, VState):
|
||||
|
||||
# get transform from earth to body frame
|
||||
q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
|
||||
R_to_body = quat_to_rot(q_att).T
|
||||
R_to_body = state_to_rot3(state).inverse()
|
||||
|
||||
# the innovation is the error between measured acceleration
|
||||
# and predicted (body frame), assuming no body acceleration
|
||||
|
||||
@@ -38,44 +38,6 @@ import symforce.symbolic as sf
|
||||
|
||||
import re
|
||||
|
||||
# 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 = sf.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 quat_to_rot_simplified(q):
|
||||
q0 = q[0]
|
||||
q1 = q[1]
|
||||
q2 = q[2]
|
||||
q3 = q[3]
|
||||
|
||||
# Use the simplified formula for unit quaternion to rotation matrix
|
||||
# as it produces a simpler and more stable EKF derivation given
|
||||
# the additional constraint: q0^2 + q1^2 + q2^2 + q3^2 = 1
|
||||
Rot = sf.Matrix([[1 - 2*q2**2 - 2*q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
|
||||
[2*(q1*q2 + q0*q3), 1 - 2*q1**2 - 2*q3**2, 2*(q2*q3 - q0*q1)],
|
||||
[2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), 1 - 2*q1**2 - 2*q2**2]])
|
||||
|
||||
return Rot
|
||||
|
||||
def quat_mult(p,q):
|
||||
r = sf.Matrix([p[0] * q[0] - p[1] * q[1] - p[2] * q[2] - p[3] * q[3],
|
||||
p[0] * q[1] + p[1] * q[0] + p[2] * q[3] - p[3] * q[2],
|
||||
p[0] * q[2] - p[1] * q[3] + p[2] * q[0] + p[3] * q[1],
|
||||
p[0] * q[3] + p[1] * q[2] - p[2] * q[1] + p[3] * q[0]])
|
||||
|
||||
return r
|
||||
|
||||
def sign_no_zero(x) -> sf.Scalar:
|
||||
"""
|
||||
Returns -1 if x is negative, 1 if x is positive, and 1 if x is zero
|
||||
|
||||
+124
-127
@@ -34,136 +34,133 @@ void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
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
|
||||
// Total ops: 398
|
||||
|
||||
// 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;
|
||||
// Intermediate terms (77)
|
||||
const Scalar _tmp0 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp1 = 4 * _tmp0;
|
||||
const Scalar _tmp2 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp3 = 2 * state(0, 0);
|
||||
const Scalar _tmp4 = _tmp2 * _tmp3;
|
||||
const Scalar _tmp5 = 2 * state(6, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(1, 0);
|
||||
const Scalar _tmp7 = -_tmp1 * state(3, 0) + _tmp4 + _tmp6;
|
||||
const Scalar _tmp8 = 2 * state(2, 0);
|
||||
const Scalar _tmp9 = _tmp2 * _tmp8;
|
||||
const Scalar _tmp10 = 2 * state(1, 0);
|
||||
const Scalar _tmp11 = _tmp0 * _tmp10;
|
||||
const Scalar _tmp12 = _tmp8 * state(3, 0);
|
||||
const Scalar _tmp13 = _tmp3 * state(1, 0);
|
||||
const Scalar _tmp14 = _tmp12 - _tmp13;
|
||||
const Scalar _tmp15 = _tmp8 * state(0, 0);
|
||||
const Scalar _tmp16 = _tmp10 * state(3, 0);
|
||||
const Scalar _tmp17 = _tmp15 + _tmp16;
|
||||
const Scalar _tmp18 = -2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp19 = 1 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp20 = _tmp18 + _tmp19;
|
||||
const Scalar _tmp21 = _tmp0 * _tmp17 + _tmp14 * _tmp2 + _tmp20 * state(6, 0);
|
||||
const Scalar _tmp22 = 2 * _tmp21;
|
||||
const Scalar _tmp23 = _tmp0 * _tmp3;
|
||||
const Scalar _tmp24 = 4 * _tmp2;
|
||||
const Scalar _tmp25 = _tmp8 * state(6, 0);
|
||||
const Scalar _tmp26 = -2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp27 = _tmp19 + _tmp26;
|
||||
const Scalar _tmp28 = _tmp3 * state(3, 0);
|
||||
const Scalar _tmp29 = _tmp8 * state(1, 0);
|
||||
const Scalar _tmp30 = -_tmp28 + _tmp29;
|
||||
const Scalar _tmp31 = _tmp12 + _tmp13;
|
||||
const Scalar _tmp32 = _tmp0 * _tmp30 + _tmp2 * _tmp27 + _tmp31 * state(6, 0);
|
||||
const Scalar _tmp33 = 2 * _tmp32;
|
||||
const Scalar _tmp34 = _tmp18 + _tmp26 + 1;
|
||||
const Scalar _tmp35 = _tmp28 + _tmp29;
|
||||
const Scalar _tmp36 = -_tmp15 + _tmp16;
|
||||
const Scalar _tmp37 = _tmp0 * _tmp34 + _tmp2 * _tmp35 + _tmp36 * state(6, 0);
|
||||
const Scalar _tmp38 = 2 * _tmp37;
|
||||
const Scalar _tmp39 = std::sqrt(Scalar(std::pow(_tmp21, Scalar(2)) + std::pow(_tmp32, Scalar(2)) +
|
||||
std::pow(_tmp37, Scalar(2)) + epsilon));
|
||||
const Scalar _tmp40 = cd * rho;
|
||||
const Scalar _tmp41 = Scalar(0.25) * _tmp37 * _tmp40 / _tmp39;
|
||||
const Scalar _tmp42 = Scalar(0.5) * _tmp39 * _tmp40;
|
||||
const Scalar _tmp43 =
|
||||
-_tmp41 * (_tmp22 * (_tmp11 + _tmp9) + _tmp33 * (-_tmp23 - _tmp24 * state(3, 0) + _tmp25) +
|
||||
_tmp38 * _tmp7) -
|
||||
_tmp42 * _tmp7 - _tmp7 * cm;
|
||||
const Scalar _tmp44 = _tmp35 * cm;
|
||||
const Scalar _tmp45 = _tmp35 * _tmp38;
|
||||
const Scalar _tmp46 = _tmp14 * _tmp22;
|
||||
const Scalar _tmp47 = _tmp27 * _tmp33;
|
||||
const Scalar _tmp48 = _tmp35 * _tmp42;
|
||||
const Scalar _tmp49 = -_tmp41 * (-_tmp45 - _tmp46 - _tmp47) + _tmp44 + _tmp48;
|
||||
const Scalar _tmp50 = -_tmp41 * (_tmp45 + _tmp46 + _tmp47) - _tmp44 - _tmp48;
|
||||
const Scalar _tmp51 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp52 = _tmp51 + _tmp9;
|
||||
const Scalar _tmp53 = 2 * state(3, 0);
|
||||
const Scalar _tmp54 = _tmp0 * _tmp53;
|
||||
const Scalar _tmp55 = 4 * state(6, 0);
|
||||
const Scalar _tmp56 = _tmp0 * _tmp8;
|
||||
const Scalar _tmp57 = _tmp3 * state(6, 0);
|
||||
const Scalar _tmp58 =
|
||||
-_tmp41 * (_tmp22 * (-_tmp4 + _tmp54 - _tmp55 * state(1, 0)) +
|
||||
_tmp33 * (-_tmp24 * state(1, 0) + _tmp56 + _tmp57) + _tmp38 * _tmp52) -
|
||||
_tmp42 * _tmp52 - _tmp52 * cm;
|
||||
const Scalar _tmp59 = _tmp10 * _tmp2;
|
||||
const Scalar _tmp60 = -_tmp1 * state(2, 0) - _tmp57 + _tmp59;
|
||||
const Scalar _tmp61 = _tmp2 * _tmp53;
|
||||
const Scalar _tmp62 = -_tmp41 * (_tmp22 * (_tmp23 - _tmp55 * state(2, 0) + _tmp61) +
|
||||
_tmp33 * (_tmp11 + _tmp51) + _tmp38 * _tmp60) -
|
||||
_tmp42 * _tmp60 - _tmp60 * cm;
|
||||
const Scalar _tmp63 = _tmp34 * cm;
|
||||
const Scalar _tmp64 = _tmp34 * _tmp38;
|
||||
const Scalar _tmp65 = _tmp17 * _tmp22;
|
||||
const Scalar _tmp66 = _tmp30 * _tmp33;
|
||||
const Scalar _tmp67 = _tmp34 * _tmp42;
|
||||
const Scalar _tmp68 = -_tmp41 * (-_tmp64 - _tmp65 - _tmp66) + _tmp63 + _tmp67;
|
||||
const Scalar _tmp69 = -_tmp25 + _tmp61;
|
||||
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));
|
||||
-_tmp41 * (_tmp22 * (_tmp56 - _tmp59) + _tmp33 * (-_tmp54 + _tmp6) + _tmp38 * _tmp69) -
|
||||
_tmp42 * _tmp69 - _tmp69 * cm;
|
||||
const Scalar _tmp71 = -_tmp41 * (_tmp64 + _tmp65 + _tmp66) - _tmp63 - _tmp67;
|
||||
const Scalar _tmp72 = -_tmp36 * _tmp42 - _tmp36 * cm -
|
||||
_tmp41 * (_tmp20 * _tmp22 + _tmp31 * _tmp33 + _tmp36 * _tmp38);
|
||||
const Scalar _tmp73 = P(23, 23) * _tmp49;
|
||||
const Scalar _tmp74 = P(22, 22) * _tmp68;
|
||||
const Scalar _tmp75 = R +
|
||||
_tmp43 * (P(0, 3) * _tmp70 + P(1, 3) * _tmp58 + P(2, 3) * _tmp62 +
|
||||
P(22, 3) * _tmp68 + P(23, 3) * _tmp49 + P(3, 3) * _tmp43 +
|
||||
P(4, 3) * _tmp71 + P(5, 3) * _tmp50 + P(6, 3) * _tmp72) +
|
||||
_tmp49 * (P(0, 23) * _tmp70 + P(1, 23) * _tmp58 + P(2, 23) * _tmp62 +
|
||||
P(22, 23) * _tmp68 + P(3, 23) * _tmp43 + P(4, 23) * _tmp71 +
|
||||
P(5, 23) * _tmp50 + P(6, 23) * _tmp72 + _tmp73) +
|
||||
_tmp50 * (P(0, 5) * _tmp70 + P(1, 5) * _tmp58 + P(2, 5) * _tmp62 +
|
||||
P(22, 5) * _tmp68 + P(23, 5) * _tmp49 + P(3, 5) * _tmp43 +
|
||||
P(4, 5) * _tmp71 + P(5, 5) * _tmp50 + P(6, 5) * _tmp72) +
|
||||
_tmp58 * (P(0, 1) * _tmp70 + P(1, 1) * _tmp58 + P(2, 1) * _tmp62 +
|
||||
P(22, 1) * _tmp68 + P(23, 1) * _tmp49 + P(3, 1) * _tmp43 +
|
||||
P(4, 1) * _tmp71 + P(5, 1) * _tmp50 + P(6, 1) * _tmp72) +
|
||||
_tmp62 * (P(0, 2) * _tmp70 + P(1, 2) * _tmp58 + P(2, 2) * _tmp62 +
|
||||
P(22, 2) * _tmp68 + P(23, 2) * _tmp49 + P(3, 2) * _tmp43 +
|
||||
P(4, 2) * _tmp71 + P(5, 2) * _tmp50 + P(6, 2) * _tmp72) +
|
||||
_tmp68 * (P(0, 22) * _tmp70 + P(1, 22) * _tmp58 + P(2, 22) * _tmp62 +
|
||||
P(23, 22) * _tmp49 + P(3, 22) * _tmp43 + P(4, 22) * _tmp71 +
|
||||
P(5, 22) * _tmp50 + P(6, 22) * _tmp72 + _tmp74) +
|
||||
_tmp70 * (P(0, 0) * _tmp70 + P(1, 0) * _tmp58 + P(2, 0) * _tmp62 +
|
||||
P(22, 0) * _tmp68 + P(23, 0) * _tmp49 + P(3, 0) * _tmp43 +
|
||||
P(4, 0) * _tmp71 + P(5, 0) * _tmp50 + P(6, 0) * _tmp72) +
|
||||
_tmp71 * (P(0, 4) * _tmp70 + P(1, 4) * _tmp58 + P(2, 4) * _tmp62 +
|
||||
P(22, 4) * _tmp68 + P(23, 4) * _tmp49 + P(3, 4) * _tmp43 +
|
||||
P(4, 4) * _tmp71 + P(5, 4) * _tmp50 + P(6, 4) * _tmp72) +
|
||||
_tmp72 * (P(0, 6) * _tmp70 + P(1, 6) * _tmp58 + P(2, 6) * _tmp62 +
|
||||
P(22, 6) * _tmp68 + P(23, 6) * _tmp49 + P(3, 6) * _tmp43 +
|
||||
P(4, 6) * _tmp71 + P(5, 6) * _tmp50 + P(6, 6) * _tmp72);
|
||||
const Scalar _tmp76 = Scalar(1.0) / (math::max<Scalar>(_tmp75, epsilon));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = _tmp74;
|
||||
_innov_var = _tmp75;
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
@@ -171,12 +168,12 @@ void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_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);
|
||||
_k(22, 0) = _tmp76 * (P(22, 0) * _tmp70 + P(22, 1) * _tmp58 + P(22, 2) * _tmp62 +
|
||||
P(22, 23) * _tmp49 + P(22, 3) * _tmp43 + P(22, 4) * _tmp71 +
|
||||
P(22, 5) * _tmp50 + P(22, 6) * _tmp72 + _tmp74);
|
||||
_k(23, 0) = _tmp76 * (P(23, 0) * _tmp70 + P(23, 1) * _tmp58 + P(23, 2) * _tmp62 +
|
||||
P(23, 22) * _tmp68 + P(23, 3) * _tmp43 + P(23, 4) * _tmp71 +
|
||||
P(23, 5) * _tmp50 + P(23, 6) * _tmp72 + _tmp73);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+123
-124
@@ -34,130 +34,129 @@ void ComputeDragYInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
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
|
||||
// Total ops: 397
|
||||
|
||||
// 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)) -
|
||||
const Scalar _tmp0 = 2 * state(0, 0);
|
||||
const Scalar _tmp1 = _tmp0 * state(3, 0);
|
||||
const Scalar _tmp2 = 2 * state(1, 0);
|
||||
const Scalar _tmp3 = _tmp2 * state(2, 0);
|
||||
const Scalar _tmp4 = -_tmp1 + _tmp3;
|
||||
const Scalar _tmp5 = _tmp4 * cm;
|
||||
const Scalar _tmp6 = -2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp7 = -2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp8 = _tmp6 + _tmp7 + 1;
|
||||
const Scalar _tmp9 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp10 = _tmp1 + _tmp3;
|
||||
const Scalar _tmp11 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp12 = _tmp0 * state(2, 0);
|
||||
const Scalar _tmp13 = _tmp2 * state(3, 0);
|
||||
const Scalar _tmp14 = -_tmp12 + _tmp13;
|
||||
const Scalar _tmp15 = _tmp10 * _tmp11 + _tmp14 * state(6, 0) + _tmp8 * _tmp9;
|
||||
const Scalar _tmp16 = 2 * state(2, 0);
|
||||
const Scalar _tmp17 = _tmp16 * state(3, 0);
|
||||
const Scalar _tmp18 = _tmp2 * state(0, 0);
|
||||
const Scalar _tmp19 = _tmp17 - _tmp18;
|
||||
const Scalar _tmp20 = _tmp12 + _tmp13;
|
||||
const Scalar _tmp21 = 1 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp22 = _tmp21 + _tmp7;
|
||||
const Scalar _tmp23 = _tmp11 * _tmp19 + _tmp20 * _tmp9 + _tmp22 * state(6, 0);
|
||||
const Scalar _tmp24 = _tmp21 + _tmp6;
|
||||
const Scalar _tmp25 = _tmp17 + _tmp18;
|
||||
const Scalar _tmp26 = _tmp11 * _tmp24 + _tmp25 * state(6, 0) + _tmp4 * _tmp9;
|
||||
const Scalar _tmp27 = std::sqrt(Scalar(std::pow(_tmp15, Scalar(2)) + std::pow(_tmp23, Scalar(2)) +
|
||||
std::pow(_tmp26, Scalar(2)) + epsilon));
|
||||
const Scalar _tmp28 = cd * rho;
|
||||
const Scalar _tmp29 = Scalar(0.5) * _tmp27 * _tmp28;
|
||||
const Scalar _tmp30 = _tmp29 * _tmp4;
|
||||
const Scalar _tmp31 = 2 * _tmp15;
|
||||
const Scalar _tmp32 = _tmp31 * _tmp8;
|
||||
const Scalar _tmp33 = 2 * _tmp23;
|
||||
const Scalar _tmp34 = _tmp20 * _tmp33;
|
||||
const Scalar _tmp35 = 2 * _tmp26;
|
||||
const Scalar _tmp36 = _tmp35 * _tmp4;
|
||||
const Scalar _tmp37 = Scalar(0.25) * _tmp26 * _tmp28 / _tmp27;
|
||||
const Scalar _tmp38 = -_tmp30 - _tmp37 * (_tmp32 + _tmp34 + _tmp36) - _tmp5;
|
||||
const Scalar _tmp39 = -_tmp25 * _tmp29 - _tmp25 * cm -
|
||||
_tmp37 * (_tmp14 * _tmp31 + _tmp22 * _tmp33 + _tmp25 * _tmp35);
|
||||
const Scalar _tmp40 = 2 * state(3, 0);
|
||||
const Scalar _tmp41 = _tmp40 * _tmp9;
|
||||
const Scalar _tmp42 = _tmp2 * state(6, 0);
|
||||
const Scalar _tmp43 = -_tmp41 + _tmp42;
|
||||
const Scalar _tmp44 = _tmp11 * _tmp2;
|
||||
const Scalar _tmp45 = _tmp16 * _tmp9;
|
||||
const Scalar _tmp46 = _tmp11 * _tmp40;
|
||||
const Scalar _tmp47 = _tmp16 * state(6, 0);
|
||||
const Scalar _tmp48 =
|
||||
-_tmp29 * _tmp43 -
|
||||
_tmp37 * (_tmp31 * (_tmp46 - _tmp47) + _tmp33 * (-_tmp44 + _tmp45) + _tmp35 * _tmp43) -
|
||||
_tmp43 * cm;
|
||||
const Scalar _tmp49 = _tmp2 * _tmp9;
|
||||
const Scalar _tmp50 = _tmp40 * state(6, 0);
|
||||
const Scalar _tmp51 = _tmp49 + _tmp50;
|
||||
const Scalar _tmp52 = _tmp0 * _tmp9;
|
||||
const Scalar _tmp53 = 4 * state(6, 0);
|
||||
const Scalar _tmp54 = 4 * _tmp9;
|
||||
const Scalar _tmp55 = _tmp0 * state(6, 0);
|
||||
const Scalar _tmp56 =
|
||||
-_tmp29 * _tmp51 -
|
||||
_tmp37 * (_tmp31 * (_tmp44 - _tmp54 * state(2, 0) - _tmp55) +
|
||||
_tmp33 * (_tmp46 + _tmp52 - _tmp53 * state(2, 0)) + _tmp35 * _tmp51) -
|
||||
_tmp51 * cm;
|
||||
const Scalar _tmp57 = _tmp24 * cm;
|
||||
const Scalar _tmp58 = _tmp10 * _tmp31;
|
||||
const Scalar _tmp59 = _tmp19 * _tmp33;
|
||||
const Scalar _tmp60 = _tmp24 * _tmp35;
|
||||
const Scalar _tmp61 = _tmp24 * _tmp29;
|
||||
const Scalar _tmp62 = -_tmp37 * (-_tmp58 - _tmp59 - _tmp60) + _tmp57 + _tmp61;
|
||||
const Scalar _tmp63 = _tmp0 * _tmp11;
|
||||
const Scalar _tmp64 = _tmp11 * _tmp16;
|
||||
const Scalar _tmp65 = 4 * _tmp11;
|
||||
const Scalar _tmp66 = _tmp45 + _tmp55 - _tmp65 * state(1, 0);
|
||||
const Scalar _tmp67 =
|
||||
-_tmp29 * _tmp66 -
|
||||
_tmp37 * (_tmp31 * (_tmp50 + _tmp64) + _tmp33 * (_tmp41 - _tmp53 * state(1, 0) - _tmp63) +
|
||||
_tmp35 * _tmp66) -
|
||||
_tmp66 * cm;
|
||||
const Scalar _tmp68 = -_tmp37 * (_tmp58 + _tmp59 + _tmp60) - _tmp57 - _tmp61;
|
||||
const Scalar _tmp69 = _tmp47 - _tmp52 - _tmp65 * state(3, 0);
|
||||
const Scalar _tmp70 = -_tmp29 * _tmp69 -
|
||||
_tmp37 * (_tmp31 * (_tmp42 - _tmp54 * state(3, 0) + _tmp63) +
|
||||
_tmp33 * (_tmp49 + _tmp64) + _tmp35 * _tmp69) -
|
||||
_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 _tmp71 = _tmp30 - _tmp37 * (-_tmp32 - _tmp34 - _tmp36) + _tmp5;
|
||||
const Scalar _tmp72 = P(22, 22) * _tmp71;
|
||||
const Scalar _tmp73 = P(23, 23) * _tmp62;
|
||||
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);
|
||||
_tmp38 * (P(0, 4) * _tmp48 + P(1, 4) * _tmp67 + P(2, 4) * _tmp56 +
|
||||
P(22, 4) * _tmp71 + P(23, 4) * _tmp62 + P(3, 4) * _tmp70 +
|
||||
P(4, 4) * _tmp38 + P(5, 4) * _tmp68 + P(6, 4) * _tmp39) +
|
||||
_tmp39 * (P(0, 6) * _tmp48 + P(1, 6) * _tmp67 + P(2, 6) * _tmp56 +
|
||||
P(22, 6) * _tmp71 + P(23, 6) * _tmp62 + P(3, 6) * _tmp70 +
|
||||
P(4, 6) * _tmp38 + P(5, 6) * _tmp68 + P(6, 6) * _tmp39) +
|
||||
_tmp48 * (P(0, 0) * _tmp48 + P(1, 0) * _tmp67 + P(2, 0) * _tmp56 +
|
||||
P(22, 0) * _tmp71 + P(23, 0) * _tmp62 + P(3, 0) * _tmp70 +
|
||||
P(4, 0) * _tmp38 + P(5, 0) * _tmp68 + P(6, 0) * _tmp39) +
|
||||
_tmp56 * (P(0, 2) * _tmp48 + P(1, 2) * _tmp67 + P(2, 2) * _tmp56 +
|
||||
P(22, 2) * _tmp71 + P(23, 2) * _tmp62 + P(3, 2) * _tmp70 +
|
||||
P(4, 2) * _tmp38 + P(5, 2) * _tmp68 + P(6, 2) * _tmp39) +
|
||||
_tmp62 * (P(0, 23) * _tmp48 + P(1, 23) * _tmp67 + P(2, 23) * _tmp56 +
|
||||
P(22, 23) * _tmp71 + P(3, 23) * _tmp70 + P(4, 23) * _tmp38 +
|
||||
P(5, 23) * _tmp68 + P(6, 23) * _tmp39 + _tmp73) +
|
||||
_tmp67 * (P(0, 1) * _tmp48 + P(1, 1) * _tmp67 + P(2, 1) * _tmp56 +
|
||||
P(22, 1) * _tmp71 + P(23, 1) * _tmp62 + P(3, 1) * _tmp70 +
|
||||
P(4, 1) * _tmp38 + P(5, 1) * _tmp68 + P(6, 1) * _tmp39) +
|
||||
_tmp68 * (P(0, 5) * _tmp48 + P(1, 5) * _tmp67 + P(2, 5) * _tmp56 +
|
||||
P(22, 5) * _tmp71 + P(23, 5) * _tmp62 + P(3, 5) * _tmp70 +
|
||||
P(4, 5) * _tmp38 + P(5, 5) * _tmp68 + P(6, 5) * _tmp39) +
|
||||
_tmp70 * (P(0, 3) * _tmp48 + P(1, 3) * _tmp67 + P(2, 3) * _tmp56 +
|
||||
P(22, 3) * _tmp71 + P(23, 3) * _tmp62 + P(3, 3) * _tmp70 +
|
||||
P(4, 3) * _tmp38 + P(5, 3) * _tmp68 + P(6, 3) * _tmp39) +
|
||||
_tmp71 * (P(0, 22) * _tmp48 + P(1, 22) * _tmp67 + P(2, 22) * _tmp56 +
|
||||
P(23, 22) * _tmp62 + P(3, 22) * _tmp70 + P(4, 22) * _tmp38 +
|
||||
P(5, 22) * _tmp68 + P(6, 22) * _tmp39 + _tmp72);
|
||||
const Scalar _tmp75 = Scalar(1.0) / (math::max<Scalar>(_tmp74, epsilon));
|
||||
|
||||
// Output terms (2)
|
||||
@@ -172,12 +171,12 @@ void ComputeDragYInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_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);
|
||||
_k(22, 0) = _tmp75 * (P(22, 0) * _tmp48 + P(22, 1) * _tmp67 + P(22, 2) * _tmp56 +
|
||||
P(22, 23) * _tmp62 + P(22, 3) * _tmp70 + P(22, 4) * _tmp38 +
|
||||
P(22, 5) * _tmp68 + P(22, 6) * _tmp39 + _tmp72);
|
||||
_k(23, 0) = _tmp75 * (P(23, 0) * _tmp48 + P(23, 1) * _tmp67 + P(23, 2) * _tmp56 +
|
||||
P(23, 22) * _tmp71 + P(23, 3) * _tmp70 + P(23, 4) * _tmp38 +
|
||||
P(23, 5) * _tmp68 + P(23, 6) * _tmp39 + _tmp73);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+65
-67
@@ -32,43 +32,41 @@ void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
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
|
||||
// Total ops: 291
|
||||
|
||||
// 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 =
|
||||
// Intermediate terms (28)
|
||||
const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 =
|
||||
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);
|
||||
const Scalar _tmp2 = _tmp1 * (_tmp0 - 2 * std::pow(state(1, 0), Scalar(2)));
|
||||
const Scalar _tmp3 = 2 * state(1, 0);
|
||||
const Scalar _tmp4 = 2 * state(3, 0);
|
||||
const Scalar _tmp5 = _tmp4 * state(6, 0);
|
||||
const Scalar _tmp6 = _tmp1 * (_tmp3 * state(4, 0) + _tmp5);
|
||||
const Scalar _tmp7 = _tmp3 * state(6, 0);
|
||||
const Scalar _tmp8 = _tmp1 * (-_tmp4 * state(4, 0) + _tmp7);
|
||||
const Scalar _tmp9 = _tmp4 * state(0, 0);
|
||||
const Scalar _tmp10 = _tmp3 * state(2, 0);
|
||||
const Scalar _tmp11 = _tmp1 * (_tmp10 - _tmp9);
|
||||
const Scalar _tmp12 = 2 * state(0, 0);
|
||||
const Scalar _tmp13 = 4 * state(5, 0);
|
||||
const Scalar _tmp14 = 2 * state(2, 0);
|
||||
const Scalar _tmp15 = _tmp14 * state(6, 0);
|
||||
const Scalar _tmp16 = _tmp1 * (-_tmp12 * state(4, 0) - _tmp13 * state(3, 0) + _tmp15);
|
||||
const Scalar _tmp17 = _tmp12 * state(6, 0);
|
||||
const Scalar _tmp18 = _tmp1 * (-_tmp13 * state(1, 0) + _tmp14 * state(4, 0) + _tmp17);
|
||||
const Scalar _tmp19 = _tmp1 * (_tmp12 * state(1, 0) + _tmp4 * state(2, 0));
|
||||
const Scalar _tmp20 = _tmp1 * (_tmp0 - 2 * std::pow(state(2, 0), Scalar(2)));
|
||||
const Scalar _tmp21 = _tmp1 * (_tmp10 + _tmp9);
|
||||
const Scalar _tmp22 = _tmp1 * (-_tmp12 * state(2, 0) + _tmp4 * state(1, 0));
|
||||
const Scalar _tmp23 = 4 * state(4, 0);
|
||||
const Scalar _tmp24 = _tmp1 * (_tmp12 * state(5, 0) - _tmp23 * state(3, 0) + _tmp7);
|
||||
const Scalar _tmp25 = _tmp1 * (-_tmp17 - _tmp23 * state(2, 0) + _tmp3 * state(5, 0));
|
||||
const Scalar _tmp26 = _tmp1 * (-_tmp15 + _tmp4 * state(5, 0));
|
||||
const Scalar _tmp27 = _tmp1 * (_tmp14 * state(5, 0) + _tmp5);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
@@ -76,36 +74,36 @@ void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_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);
|
||||
_tmp11 * (P(0, 4) * _tmp8 + P(1, 4) * _tmp18 + P(2, 4) * _tmp6 + P(3, 4) * _tmp16 +
|
||||
P(4, 4) * _tmp11 + P(5, 4) * _tmp2 + P(6, 4) * _tmp19) +
|
||||
_tmp16 * (P(0, 3) * _tmp8 + P(1, 3) * _tmp18 + P(2, 3) * _tmp6 + P(3, 3) * _tmp16 +
|
||||
P(4, 3) * _tmp11 + P(5, 3) * _tmp2 + P(6, 3) * _tmp19) +
|
||||
_tmp18 * (P(0, 1) * _tmp8 + P(1, 1) * _tmp18 + P(2, 1) * _tmp6 + P(3, 1) * _tmp16 +
|
||||
P(4, 1) * _tmp11 + P(5, 1) * _tmp2 + P(6, 1) * _tmp19) +
|
||||
_tmp19 * (P(0, 6) * _tmp8 + P(1, 6) * _tmp18 + P(2, 6) * _tmp6 + P(3, 6) * _tmp16 +
|
||||
P(4, 6) * _tmp11 + P(5, 6) * _tmp2 + P(6, 6) * _tmp19) +
|
||||
_tmp2 * (P(0, 5) * _tmp8 + P(1, 5) * _tmp18 + P(2, 5) * _tmp6 + P(3, 5) * _tmp16 +
|
||||
P(4, 5) * _tmp11 + P(5, 5) * _tmp2 + P(6, 5) * _tmp19) +
|
||||
_tmp6 * (P(0, 2) * _tmp8 + P(1, 2) * _tmp18 + P(2, 2) * _tmp6 + P(3, 2) * _tmp16 +
|
||||
P(4, 2) * _tmp11 + P(5, 2) * _tmp2 + P(6, 2) * _tmp19) +
|
||||
_tmp8 * (P(0, 0) * _tmp8 + P(1, 0) * _tmp18 + P(2, 0) * _tmp6 + P(3, 0) * _tmp16 +
|
||||
P(4, 0) * _tmp11 + P(5, 0) * _tmp2 + P(6, 0) * _tmp19);
|
||||
_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);
|
||||
_tmp20 * (-P(0, 4) * _tmp26 - P(1, 4) * _tmp27 - P(2, 4) * _tmp25 - P(3, 4) * _tmp24 -
|
||||
P(4, 4) * _tmp20 - P(5, 4) * _tmp21 - P(6, 4) * _tmp22) -
|
||||
_tmp21 * (-P(0, 5) * _tmp26 - P(1, 5) * _tmp27 - P(2, 5) * _tmp25 - P(3, 5) * _tmp24 -
|
||||
P(4, 5) * _tmp20 - P(5, 5) * _tmp21 - P(6, 5) * _tmp22) -
|
||||
_tmp22 * (-P(0, 6) * _tmp26 - P(1, 6) * _tmp27 - P(2, 6) * _tmp25 - P(3, 6) * _tmp24 -
|
||||
P(4, 6) * _tmp20 - P(5, 6) * _tmp21 - P(6, 6) * _tmp22) -
|
||||
_tmp24 * (-P(0, 3) * _tmp26 - P(1, 3) * _tmp27 - P(2, 3) * _tmp25 - P(3, 3) * _tmp24 -
|
||||
P(4, 3) * _tmp20 - P(5, 3) * _tmp21 - P(6, 3) * _tmp22) -
|
||||
_tmp25 * (-P(0, 2) * _tmp26 - P(1, 2) * _tmp27 - P(2, 2) * _tmp25 - P(3, 2) * _tmp24 -
|
||||
P(4, 2) * _tmp20 - P(5, 2) * _tmp21 - P(6, 2) * _tmp22) -
|
||||
_tmp26 * (-P(0, 0) * _tmp26 - P(1, 0) * _tmp27 - P(2, 0) * _tmp25 - P(3, 0) * _tmp24 -
|
||||
P(4, 0) * _tmp20 - P(5, 0) * _tmp21 - P(6, 0) * _tmp22) -
|
||||
_tmp27 * (-P(0, 1) * _tmp26 - P(1, 1) * _tmp27 - P(2, 1) * _tmp25 - P(3, 1) * _tmp24 -
|
||||
P(4, 1) * _tmp20 - P(5, 1) * _tmp21 - P(6, 1) * _tmp22);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
@@ -113,13 +111,13 @@ void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_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;
|
||||
_h(0, 0) = _tmp8;
|
||||
_h(1, 0) = _tmp18;
|
||||
_h(2, 0) = _tmp6;
|
||||
_h(3, 0) = _tmp16;
|
||||
_h(4, 0) = _tmp11;
|
||||
_h(5, 0) = _tmp2;
|
||||
_h(6, 0) = _tmp19;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+31
-32
@@ -32,7 +32,7 @@ void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const Scalar R, const Scalar epsilon,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 171
|
||||
// Total ops: 166
|
||||
|
||||
// Input arrays
|
||||
|
||||
@@ -41,39 +41,38 @@ void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
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));
|
||||
_tmp0 * (-2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1);
|
||||
const Scalar _tmp2 = 2 * state(3, 0);
|
||||
const Scalar _tmp3 = 2 * state(2, 0);
|
||||
const Scalar _tmp4 = _tmp0 * (_tmp2 * state(0, 0) + _tmp3 * state(1, 0));
|
||||
const Scalar _tmp5 = _tmp0 * (_tmp2 * state(1, 0) - _tmp3 * state(0, 0));
|
||||
const Scalar _tmp6 = 4 * state(4, 0);
|
||||
const Scalar _tmp7 = 2 * state(5, 0);
|
||||
const Scalar _tmp8 = 2 * state(6, 0);
|
||||
const Scalar _tmp9 = _tmp0 * (-_tmp6 * state(3, 0) + _tmp7 * state(0, 0) + _tmp8 * state(1, 0));
|
||||
const Scalar _tmp10 = _tmp0 * (-_tmp6 * state(2, 0) + _tmp7 * state(1, 0) - _tmp8 * state(0, 0));
|
||||
const Scalar _tmp11 = _tmp0 * (-_tmp3 * state(6, 0) + _tmp7 * state(3, 0));
|
||||
const Scalar _tmp12 = _tmp0 * (_tmp2 * state(6, 0) + _tmp3 * state(5, 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);
|
||||
_tmp1 * (-P(0, 4) * _tmp11 - P(1, 4) * _tmp12 - P(2, 4) * _tmp10 -
|
||||
P(3, 4) * _tmp9 - P(4, 4) * _tmp1 - P(5, 4) * _tmp4 - P(6, 4) * _tmp5) -
|
||||
_tmp10 * (-P(0, 2) * _tmp11 - P(1, 2) * _tmp12 - P(2, 2) * _tmp10 -
|
||||
P(3, 2) * _tmp9 - P(4, 2) * _tmp1 - P(5, 2) * _tmp4 - P(6, 2) * _tmp5) -
|
||||
_tmp11 * (-P(0, 0) * _tmp11 - P(1, 0) * _tmp12 - P(2, 0) * _tmp10 -
|
||||
P(3, 0) * _tmp9 - P(4, 0) * _tmp1 - P(5, 0) * _tmp4 - P(6, 0) * _tmp5) -
|
||||
_tmp12 * (-P(0, 1) * _tmp11 - P(1, 1) * _tmp12 - P(2, 1) * _tmp10 -
|
||||
P(3, 1) * _tmp9 - P(4, 1) * _tmp1 - P(5, 1) * _tmp4 - P(6, 1) * _tmp5) -
|
||||
_tmp4 * (-P(0, 5) * _tmp11 - P(1, 5) * _tmp12 - P(2, 5) * _tmp10 -
|
||||
P(3, 5) * _tmp9 - P(4, 5) * _tmp1 - P(5, 5) * _tmp4 - P(6, 5) * _tmp5) -
|
||||
_tmp5 * (-P(0, 6) * _tmp11 - P(1, 6) * _tmp12 - P(2, 6) * _tmp10 -
|
||||
P(3, 6) * _tmp9 - P(4, 6) * _tmp1 - P(5, 6) * _tmp4 - P(6, 6) * _tmp5) -
|
||||
_tmp9 * (-P(0, 3) * _tmp11 - P(1, 3) * _tmp12 - P(2, 3) * _tmp10 -
|
||||
P(3, 3) * _tmp9 - P(4, 3) * _tmp1 - P(5, 3) * _tmp4 - P(6, 3) * _tmp5);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
@@ -81,10 +80,10 @@ void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = -_tmp10;
|
||||
_h(1, 0) = -_tmp11;
|
||||
_h(2, 0) = -_tmp12;
|
||||
_h(3, 0) = -_tmp8;
|
||||
_h(0, 0) = -_tmp11;
|
||||
_h(1, 0) = -_tmp12;
|
||||
_h(2, 0) = -_tmp10;
|
||||
_h(3, 0) = -_tmp9;
|
||||
_h(4, 0) = -_tmp1;
|
||||
_h(5, 0) = -_tmp4;
|
||||
_h(6, 0) = -_tmp5;
|
||||
|
||||
+38
-37
@@ -34,53 +34,54 @@ void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const Scalar epsilon, Scalar* const meas_pred = nullptr,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 101
|
||||
// Total ops: 105
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (26)
|
||||
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 = std::sin(antenna_yaw_offset);
|
||||
const Scalar _tmp4 = state(0, 0) * state(3, 0);
|
||||
const Scalar _tmp5 = state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp6 = std::cos(antenna_yaw_offset);
|
||||
const Scalar _tmp7 = 2 * _tmp6;
|
||||
const Scalar _tmp8 = _tmp3 * (_tmp0 - _tmp1 + _tmp2) + _tmp7 * (_tmp4 + _tmp5);
|
||||
const Scalar _tmp9 = 2 * _tmp3;
|
||||
const Scalar _tmp10 = _tmp6 * (-_tmp0 + _tmp1 + _tmp2) + _tmp9 * (-_tmp4 + _tmp5);
|
||||
const Scalar _tmp11 = _tmp10 + epsilon * ((((_tmp10) > 0) - ((_tmp10) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp12 = _tmp7 * state(0, 0) - _tmp9 * state(3, 0);
|
||||
const Scalar _tmp13 = Scalar(1.0) / (_tmp11);
|
||||
const Scalar _tmp14 = _tmp7 * state(3, 0);
|
||||
const Scalar _tmp15 = _tmp9 * state(0, 0);
|
||||
const Scalar _tmp16 = std::pow(_tmp11, Scalar(2));
|
||||
const Scalar _tmp17 = _tmp8 / _tmp16;
|
||||
const Scalar _tmp18 = _tmp16 / (_tmp16 + std::pow(_tmp8, Scalar(2)));
|
||||
const Scalar _tmp19 = _tmp18 * (_tmp12 * _tmp13 - _tmp17 * (-_tmp14 - _tmp15));
|
||||
const Scalar _tmp20 = _tmp7 * state(1, 0) + _tmp9 * state(2, 0);
|
||||
const Scalar _tmp21 = _tmp9 * state(1, 0);
|
||||
const Scalar _tmp22 = _tmp7 * state(2, 0);
|
||||
const Scalar _tmp23 = _tmp18 * (_tmp13 * (-_tmp21 + _tmp22) - _tmp17 * _tmp20);
|
||||
const Scalar _tmp24 = _tmp18 * (-_tmp12 * _tmp17 + _tmp13 * (_tmp14 + _tmp15));
|
||||
const Scalar _tmp25 = _tmp18 * (_tmp13 * _tmp20 - _tmp17 * (_tmp21 - _tmp22));
|
||||
// Intermediate terms (22)
|
||||
const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::sin(antenna_yaw_offset);
|
||||
const Scalar _tmp2 = 2 * state(0, 0) * state(3, 0);
|
||||
const Scalar _tmp3 = 2 * state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp4 = std::cos(antenna_yaw_offset);
|
||||
const Scalar _tmp5 =
|
||||
_tmp1 * (_tmp0 - 2 * std::pow(state(1, 0), Scalar(2))) + _tmp4 * (_tmp2 + _tmp3);
|
||||
const Scalar _tmp6 =
|
||||
_tmp1 * (-_tmp2 + _tmp3) + _tmp4 * (_tmp0 - 2 * std::pow(state(2, 0), Scalar(2)));
|
||||
const Scalar _tmp7 = _tmp6 + epsilon * ((((_tmp6) > 0) - ((_tmp6) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp8 = 4 * _tmp1;
|
||||
const Scalar _tmp9 = 2 * _tmp4;
|
||||
const Scalar _tmp10 = Scalar(1.0) / (_tmp7);
|
||||
const Scalar _tmp11 = 4 * _tmp4;
|
||||
const Scalar _tmp12 = 2 * _tmp1;
|
||||
const Scalar _tmp13 = std::pow(_tmp7, Scalar(2));
|
||||
const Scalar _tmp14 = _tmp5 / _tmp13;
|
||||
const Scalar _tmp15 = _tmp13 / (_tmp13 + std::pow(_tmp5, Scalar(2)));
|
||||
const Scalar _tmp16 = _tmp15 * (_tmp10 * (-_tmp8 * state(3, 0) + _tmp9 * state(0, 0)) -
|
||||
_tmp14 * (-_tmp11 * state(3, 0) - _tmp12 * state(0, 0)));
|
||||
const Scalar _tmp17 = _tmp12 * _tmp14;
|
||||
const Scalar _tmp18 = _tmp10 * _tmp9;
|
||||
const Scalar _tmp19 = _tmp15 * (_tmp17 * state(3, 0) + _tmp18 * state(3, 0));
|
||||
const Scalar _tmp20 =
|
||||
_tmp15 * (-_tmp14 * (-_tmp11 * state(2, 0) + _tmp12 * state(1, 0)) + _tmp18 * state(1, 0));
|
||||
const Scalar _tmp21 =
|
||||
_tmp15 * (_tmp10 * (-_tmp8 * state(1, 0) + _tmp9 * state(2, 0)) - _tmp17 * state(2, 0));
|
||||
|
||||
// Output terms (3)
|
||||
if (meas_pred != nullptr) {
|
||||
Scalar& _meas_pred = (*meas_pred);
|
||||
|
||||
_meas_pred = std::atan2(_tmp8, _tmp11);
|
||||
_meas_pred = std::atan2(_tmp5, _tmp7);
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var =
|
||||
R + _tmp19 * (P(0, 3) * _tmp24 + P(1, 3) * _tmp23 + P(2, 3) * _tmp25 + P(3, 3) * _tmp19) +
|
||||
_tmp23 * (P(0, 1) * _tmp24 + P(1, 1) * _tmp23 + P(2, 1) * _tmp25 + P(3, 1) * _tmp19) +
|
||||
_tmp24 * (P(0, 0) * _tmp24 + P(1, 0) * _tmp23 + P(2, 0) * _tmp25 + P(3, 0) * _tmp19) +
|
||||
_tmp25 * (P(0, 2) * _tmp24 + P(1, 2) * _tmp23 + P(2, 2) * _tmp25 + P(3, 2) * _tmp19);
|
||||
R + _tmp16 * (P(0, 3) * _tmp19 + P(1, 3) * _tmp21 + P(2, 3) * _tmp20 + P(3, 3) * _tmp16) +
|
||||
_tmp19 * (P(0, 0) * _tmp19 + P(1, 0) * _tmp21 + P(2, 0) * _tmp20 + P(3, 0) * _tmp16) +
|
||||
_tmp20 * (P(0, 2) * _tmp19 + P(1, 2) * _tmp21 + P(2, 2) * _tmp20 + P(3, 2) * _tmp16) +
|
||||
_tmp21 * (P(0, 1) * _tmp19 + P(1, 1) * _tmp21 + P(2, 1) * _tmp20 + P(3, 1) * _tmp16);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
@@ -88,10 +89,10 @@ void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp24;
|
||||
_h(1, 0) = _tmp23;
|
||||
_h(2, 0) = _tmp25;
|
||||
_h(3, 0) = _tmp19;
|
||||
_h(0, 0) = _tmp19;
|
||||
_h(1, 0) = _tmp21;
|
||||
_h(2, 0) = _tmp20;
|
||||
_h(3, 0) = _tmp16;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+127
-171
@@ -39,236 +39,192 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
matrix::Matrix<Scalar, 24, 1>* const Kx = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const Ky = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const Kz = nullptr) {
|
||||
// Total ops: 736
|
||||
// Total ops: 617
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (54)
|
||||
const Scalar _tmp0 =
|
||||
// Intermediate terms (34)
|
||||
const Scalar _tmp0 = 2 * state(2, 0);
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 =
|
||||
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 _tmp1 = Scalar(19.613299999999999) * state(1, 0);
|
||||
const Scalar _tmp2 = -P(3, 0) * _tmp1;
|
||||
const Scalar _tmp3 = Scalar(19.613299999999999) * state(2, 0);
|
||||
const Scalar _tmp4 = P(0, 0) * _tmp3;
|
||||
const Scalar _tmp5 = Scalar(19.613299999999999) * state(0, 0);
|
||||
const Scalar _tmp6 = P(2, 0) * _tmp5;
|
||||
const Scalar _tmp7 = Scalar(19.613299999999999) * state(3, 0);
|
||||
const Scalar _tmp8 = P(3, 1) * _tmp1;
|
||||
const Scalar _tmp9 = P(2, 1) * _tmp5;
|
||||
const Scalar _tmp10 = -P(1, 1) * _tmp7;
|
||||
const Scalar _tmp11 = P(0, 2) * _tmp3;
|
||||
const Scalar _tmp12 = P(2, 2) * _tmp5;
|
||||
const Scalar _tmp13 = -P(1, 2) * _tmp7;
|
||||
const Scalar _tmp14 = -P(3, 3) * _tmp1;
|
||||
const Scalar _tmp15 = P(0, 3) * _tmp3;
|
||||
const Scalar _tmp16 = -P(1, 3) * _tmp7;
|
||||
const Scalar _tmp17 = R - _tmp1 * (P(2, 3) * _tmp5 + _tmp14 + _tmp15 + _tmp16) +
|
||||
_tmp3 * (-P(1, 0) * _tmp7 + _tmp2 + _tmp4 + _tmp6) +
|
||||
_tmp5 * (-P(3, 2) * _tmp1 + _tmp11 + _tmp12 + _tmp13) -
|
||||
_tmp7 * (P(0, 1) * _tmp3 + _tmp10 - _tmp8 + _tmp9);
|
||||
const Scalar _tmp18 = P(3, 0) * _tmp3;
|
||||
const Scalar _tmp19 = -P(0, 0) * _tmp1;
|
||||
const Scalar _tmp20 = -P(1, 0) * _tmp5;
|
||||
const Scalar _tmp21 = P(3, 2) * _tmp3;
|
||||
const Scalar _tmp22 = -P(2, 2) * _tmp7;
|
||||
const Scalar _tmp23 = P(1, 2) * _tmp5;
|
||||
const Scalar _tmp24 = P(0, 1) * _tmp1;
|
||||
const Scalar _tmp25 = -P(2, 1) * _tmp7;
|
||||
const Scalar _tmp26 = -P(1, 1) * _tmp5;
|
||||
const Scalar _tmp27 = -P(3, 3) * _tmp3;
|
||||
const Scalar _tmp28 = -P(0, 3) * _tmp1;
|
||||
const Scalar _tmp29 = -P(2, 3) * _tmp7;
|
||||
const Scalar _tmp30 = R - _tmp1 * (-P(2, 0) * _tmp7 - _tmp18 + _tmp19 + _tmp20) -
|
||||
_tmp3 * (-P(1, 3) * _tmp5 + _tmp27 + _tmp28 + _tmp29) -
|
||||
_tmp5 * (-P(3, 1) * _tmp3 - _tmp24 + _tmp25 + _tmp26) -
|
||||
_tmp7 * (-P(0, 2) * _tmp1 - _tmp21 + _tmp22 - _tmp23);
|
||||
const Scalar _tmp31 = -P(0, 0) * _tmp5;
|
||||
const Scalar _tmp32 = P(2, 0) * _tmp3;
|
||||
const Scalar _tmp33 = P(1, 0) * _tmp1;
|
||||
const Scalar _tmp34 = -P(3, 2) * _tmp7;
|
||||
const Scalar _tmp35 = P(0, 2) * _tmp5;
|
||||
const Scalar _tmp36 = P(2, 2) * _tmp3;
|
||||
const Scalar _tmp37 = -P(3, 1) * _tmp7;
|
||||
const Scalar _tmp38 = -P(0, 1) * _tmp5;
|
||||
const Scalar _tmp39 = P(1, 1) * _tmp1;
|
||||
const Scalar _tmp40 = -P(3, 3) * _tmp7;
|
||||
const Scalar _tmp41 = P(2, 3) * _tmp3;
|
||||
const Scalar _tmp42 = P(1, 3) * _tmp1;
|
||||
const Scalar _tmp43 = R + _tmp1 * (P(2, 1) * _tmp3 + _tmp37 + _tmp38 + _tmp39) +
|
||||
_tmp3 * (P(1, 2) * _tmp1 + _tmp34 - _tmp35 + _tmp36) -
|
||||
_tmp5 * (-P(3, 0) * _tmp7 + _tmp31 + _tmp32 + _tmp33) -
|
||||
_tmp7 * (-P(0, 3) * _tmp5 + _tmp40 + _tmp41 + _tmp42);
|
||||
const Scalar _tmp44 = Scalar(1.0) / (_tmp17);
|
||||
const Scalar _tmp45 = Scalar(19.613299999999999) * P(8, 3);
|
||||
const Scalar _tmp46 = Scalar(19.613299999999999) * P(8, 0);
|
||||
const Scalar _tmp47 = Scalar(19.613299999999999) * P(8, 2);
|
||||
const Scalar _tmp48 = Scalar(19.613299999999999) * P(9, 3);
|
||||
const Scalar _tmp49 = Scalar(19.613299999999999) * P(9, 2);
|
||||
const Scalar _tmp50 = Scalar(19.613299999999999) * P(9, 0);
|
||||
const Scalar _tmp51 = Scalar(1.0) / (_tmp30);
|
||||
const Scalar _tmp52 = Scalar(19.613299999999999) * P(4, 0);
|
||||
const Scalar _tmp53 = Scalar(1.0) / (_tmp43);
|
||||
const Scalar _tmp3 = Scalar(19.613299999999999) * state(1, 0);
|
||||
const Scalar _tmp4 = -P(3, 0) * _tmp3;
|
||||
const Scalar _tmp5 = Scalar(19.613299999999999) * state(2, 0);
|
||||
const Scalar _tmp6 = P(0, 0) * _tmp5;
|
||||
const Scalar _tmp7 = Scalar(19.613299999999999) * state(0, 0);
|
||||
const Scalar _tmp8 = Scalar(19.613299999999999) * state(3, 0);
|
||||
const Scalar _tmp9 = P(2, 1) * _tmp7;
|
||||
const Scalar _tmp10 = -P(1, 1) * _tmp8;
|
||||
const Scalar _tmp11 = P(2, 2) * _tmp7;
|
||||
const Scalar _tmp12 = -P(1, 2) * _tmp8;
|
||||
const Scalar _tmp13 = -P(3, 3) * _tmp3;
|
||||
const Scalar _tmp14 = P(0, 3) * _tmp5;
|
||||
const Scalar _tmp15 = R - _tmp3 * (-P(1, 3) * _tmp8 + P(2, 3) * _tmp7 + _tmp13 + _tmp14) +
|
||||
_tmp5 * (-P(1, 0) * _tmp8 + P(2, 0) * _tmp7 + _tmp4 + _tmp6) +
|
||||
_tmp7 * (P(0, 2) * _tmp5 - P(3, 2) * _tmp3 + _tmp11 + _tmp12) -
|
||||
_tmp8 * (P(0, 1) * _tmp5 - P(3, 1) * _tmp3 + _tmp10 + _tmp9);
|
||||
const Scalar _tmp16 = P(3, 0) * _tmp5;
|
||||
const Scalar _tmp17 = -P(0, 0) * _tmp3;
|
||||
const Scalar _tmp18 = -P(2, 2) * _tmp8;
|
||||
const Scalar _tmp19 = P(1, 2) * _tmp7;
|
||||
const Scalar _tmp20 = -P(2, 1) * _tmp8;
|
||||
const Scalar _tmp21 = -P(1, 1) * _tmp7;
|
||||
const Scalar _tmp22 = -P(3, 3) * _tmp5;
|
||||
const Scalar _tmp23 = -P(0, 3) * _tmp3;
|
||||
const Scalar _tmp24 = R - _tmp3 * (-P(1, 0) * _tmp7 - P(2, 0) * _tmp8 - _tmp16 + _tmp17) -
|
||||
_tmp5 * (-P(1, 3) * _tmp7 - P(2, 3) * _tmp8 + _tmp22 + _tmp23) -
|
||||
_tmp7 * (-P(0, 1) * _tmp3 - P(3, 1) * _tmp5 + _tmp20 + _tmp21) -
|
||||
_tmp8 * (-P(0, 2) * _tmp3 - P(3, 2) * _tmp5 + _tmp18 - _tmp19);
|
||||
const Scalar _tmp25 = Scalar(39.226599999999998) * state(2, 0);
|
||||
const Scalar _tmp26 = P(2, 2) * _tmp25;
|
||||
const Scalar _tmp27 = Scalar(39.226599999999998) * state(1, 0);
|
||||
const Scalar _tmp28 = P(1, 1) * _tmp27;
|
||||
const Scalar _tmp29 =
|
||||
R + _tmp25 * (P(1, 2) * _tmp27 + _tmp26) + _tmp27 * (P(2, 1) * _tmp25 + _tmp28);
|
||||
const Scalar _tmp30 = Scalar(1.0) / (_tmp15);
|
||||
const Scalar _tmp31 = Scalar(19.613299999999999) * P(6, 3);
|
||||
const Scalar _tmp32 = Scalar(1.0) / (_tmp24);
|
||||
const Scalar _tmp33 = Scalar(1.0) / (_tmp29);
|
||||
|
||||
// Output terms (5)
|
||||
if (innov != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _innov = (*innov);
|
||||
|
||||
_innov(0, 0) = -_tmp0 * meas(0, 0) + Scalar(19.613299999999999) * state(0, 0) * state(2, 0) -
|
||||
Scalar(19.613299999999999) * state(1, 0) * state(3, 0);
|
||||
_innov(1, 0) = -_tmp0 * meas(1, 0) - Scalar(19.613299999999999) * state(0, 0) * state(1, 0) -
|
||||
Scalar(19.613299999999999) * state(2, 0) * state(3, 0);
|
||||
_innov(2, 0) = -_tmp0 * meas(2, 0) -
|
||||
Scalar(9.8066499999999994) * std::pow(state(0, 0), Scalar(2)) +
|
||||
Scalar(9.8066499999999994) * std::pow(state(1, 0), Scalar(2)) +
|
||||
Scalar(9.8066499999999994) * std::pow(state(2, 0), Scalar(2)) -
|
||||
Scalar(9.8066499999999994) * std::pow(state(3, 0), Scalar(2));
|
||||
_innov(0, 0) = Scalar(9.8066499999999994) * _tmp0 * state(0, 0) -
|
||||
Scalar(9.8066499999999994) * _tmp1 * state(3, 0) - _tmp2 * meas(0, 0);
|
||||
_innov(1, 0) = -Scalar(9.8066499999999994) * _tmp0 * state(3, 0) -
|
||||
Scalar(9.8066499999999994) * _tmp1 * state(0, 0) - _tmp2 * meas(1, 0);
|
||||
_innov(2, 0) =
|
||||
-_tmp2 * meas(2, 0) + Scalar(19.613299999999999) * std::pow(state(1, 0), Scalar(2)) +
|
||||
Scalar(19.613299999999999) * std::pow(state(2, 0), Scalar(2)) + 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) = _tmp30;
|
||||
_innov_var(2, 0) = _tmp43;
|
||||
_innov_var(0, 0) = _tmp15;
|
||||
_innov_var(1, 0) = _tmp24;
|
||||
_innov_var(2, 0) = _tmp29;
|
||||
}
|
||||
|
||||
if (Kx != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _kx = (*Kx);
|
||||
|
||||
_kx(0, 0) = _tmp44 * (-P(0, 1) * _tmp7 + _tmp28 + _tmp35 + _tmp4);
|
||||
_kx(1, 0) = _tmp44 * (P(1, 0) * _tmp3 + _tmp10 + _tmp23 - _tmp42);
|
||||
_kx(2, 0) = _tmp44 * (-P(2, 3) * _tmp1 + _tmp12 + _tmp25 + _tmp32);
|
||||
_kx(3, 0) = _tmp44 * (P(3, 2) * _tmp5 + _tmp14 + _tmp18 + _tmp37);
|
||||
_kx(4, 0) = _tmp44 * (P(4, 0) * _tmp3 - P(4, 1) * _tmp7 + P(4, 2) * _tmp5 - P(4, 3) * _tmp1);
|
||||
_kx(5, 0) = _tmp44 * (P(5, 0) * _tmp3 - P(5, 1) * _tmp7 + P(5, 2) * _tmp5 - P(5, 3) * _tmp1);
|
||||
_kx(6, 0) = _tmp44 * (P(6, 0) * _tmp3 - P(6, 1) * _tmp7 + P(6, 2) * _tmp5 - P(6, 3) * _tmp1);
|
||||
_kx(7, 0) = _tmp44 * (P(7, 0) * _tmp3 - P(7, 1) * _tmp7 + P(7, 2) * _tmp5 - P(7, 3) * _tmp1);
|
||||
_kx(8, 0) = _tmp44 * (-P(8, 1) * _tmp7 - _tmp45 * state(1, 0) + _tmp46 * state(2, 0) +
|
||||
_tmp47 * state(0, 0));
|
||||
_kx(9, 0) = _tmp44 * (-P(9, 1) * _tmp7 - _tmp48 * state(1, 0) + _tmp49 * state(0, 0) +
|
||||
_tmp50 * state(2, 0));
|
||||
_kx(0, 0) = _tmp30 * (-P(0, 1) * _tmp8 + P(0, 2) * _tmp7 + _tmp23 + _tmp6);
|
||||
_kx(1, 0) = _tmp30 * (P(1, 0) * _tmp5 - P(1, 3) * _tmp3 + _tmp10 + _tmp19);
|
||||
_kx(2, 0) = _tmp30 * (P(2, 0) * _tmp5 - P(2, 3) * _tmp3 + _tmp11 + _tmp20);
|
||||
_kx(3, 0) = _tmp30 * (-P(3, 1) * _tmp8 + P(3, 2) * _tmp7 + _tmp13 + _tmp16);
|
||||
_kx(4, 0) = _tmp30 * (P(4, 0) * _tmp5 - P(4, 1) * _tmp8 + P(4, 2) * _tmp7 - P(4, 3) * _tmp3);
|
||||
_kx(5, 0) = _tmp30 * (P(5, 0) * _tmp5 - P(5, 1) * _tmp8 + P(5, 2) * _tmp7 - P(5, 3) * _tmp3);
|
||||
_kx(6, 0) =
|
||||
_tmp30 * (P(6, 0) * _tmp5 - P(6, 1) * _tmp8 + P(6, 2) * _tmp7 - _tmp31 * state(1, 0));
|
||||
_kx(7, 0) = _tmp30 * (P(7, 0) * _tmp5 - P(7, 1) * _tmp8 + P(7, 2) * _tmp7 - P(7, 3) * _tmp3);
|
||||
_kx(8, 0) = _tmp30 * (P(8, 0) * _tmp5 - P(8, 1) * _tmp8 + P(8, 2) * _tmp7 - P(8, 3) * _tmp3);
|
||||
_kx(9, 0) = _tmp30 * (P(9, 0) * _tmp5 - P(9, 1) * _tmp8 + P(9, 2) * _tmp7 - P(9, 3) * _tmp3);
|
||||
_kx(10, 0) =
|
||||
_tmp44 * (P(10, 0) * _tmp3 - P(10, 1) * _tmp7 + P(10, 2) * _tmp5 - P(10, 3) * _tmp1);
|
||||
_tmp30 * (P(10, 0) * _tmp5 - P(10, 1) * _tmp8 + P(10, 2) * _tmp7 - P(10, 3) * _tmp3);
|
||||
_kx(11, 0) =
|
||||
_tmp44 * (P(11, 0) * _tmp3 - P(11, 1) * _tmp7 + P(11, 2) * _tmp5 - P(11, 3) * _tmp1);
|
||||
_tmp30 * (P(11, 0) * _tmp5 - P(11, 1) * _tmp8 + P(11, 2) * _tmp7 - P(11, 3) * _tmp3);
|
||||
_kx(12, 0) =
|
||||
_tmp44 * (P(12, 0) * _tmp3 - P(12, 1) * _tmp7 + P(12, 2) * _tmp5 - P(12, 3) * _tmp1);
|
||||
_tmp30 * (P(12, 0) * _tmp5 - P(12, 1) * _tmp8 + P(12, 2) * _tmp7 - P(12, 3) * _tmp3);
|
||||
_kx(13, 0) =
|
||||
_tmp44 * (P(13, 0) * _tmp3 - P(13, 1) * _tmp7 + P(13, 2) * _tmp5 - P(13, 3) * _tmp1);
|
||||
_tmp30 * (P(13, 0) * _tmp5 - P(13, 1) * _tmp8 + P(13, 2) * _tmp7 - P(13, 3) * _tmp3);
|
||||
_kx(14, 0) =
|
||||
_tmp44 * (P(14, 0) * _tmp3 - P(14, 1) * _tmp7 + P(14, 2) * _tmp5 - P(14, 3) * _tmp1);
|
||||
_tmp30 * (P(14, 0) * _tmp5 - P(14, 1) * _tmp8 + P(14, 2) * _tmp7 - P(14, 3) * _tmp3);
|
||||
_kx(15, 0) =
|
||||
_tmp44 * (P(15, 0) * _tmp3 - P(15, 1) * _tmp7 + P(15, 2) * _tmp5 - P(15, 3) * _tmp1);
|
||||
_tmp30 * (P(15, 0) * _tmp5 - P(15, 1) * _tmp8 + P(15, 2) * _tmp7 - P(15, 3) * _tmp3);
|
||||
_kx(16, 0) =
|
||||
_tmp44 * (P(16, 0) * _tmp3 - P(16, 1) * _tmp7 + P(16, 2) * _tmp5 - P(16, 3) * _tmp1);
|
||||
_tmp30 * (P(16, 0) * _tmp5 - P(16, 1) * _tmp8 + P(16, 2) * _tmp7 - P(16, 3) * _tmp3);
|
||||
_kx(17, 0) =
|
||||
_tmp44 * (P(17, 0) * _tmp3 - P(17, 1) * _tmp7 + P(17, 2) * _tmp5 - P(17, 3) * _tmp1);
|
||||
_tmp30 * (P(17, 0) * _tmp5 - P(17, 1) * _tmp8 + P(17, 2) * _tmp7 - P(17, 3) * _tmp3);
|
||||
_kx(18, 0) =
|
||||
_tmp44 * (P(18, 0) * _tmp3 - P(18, 1) * _tmp7 + P(18, 2) * _tmp5 - P(18, 3) * _tmp1);
|
||||
_tmp30 * (P(18, 0) * _tmp5 - P(18, 1) * _tmp8 + P(18, 2) * _tmp7 - P(18, 3) * _tmp3);
|
||||
_kx(19, 0) =
|
||||
_tmp44 * (P(19, 0) * _tmp3 - P(19, 1) * _tmp7 + P(19, 2) * _tmp5 - P(19, 3) * _tmp1);
|
||||
_tmp30 * (P(19, 0) * _tmp5 - P(19, 1) * _tmp8 + P(19, 2) * _tmp7 - P(19, 3) * _tmp3);
|
||||
_kx(20, 0) =
|
||||
_tmp44 * (P(20, 0) * _tmp3 - P(20, 1) * _tmp7 + P(20, 2) * _tmp5 - P(20, 3) * _tmp1);
|
||||
_tmp30 * (P(20, 0) * _tmp5 - P(20, 1) * _tmp8 + P(20, 2) * _tmp7 - P(20, 3) * _tmp3);
|
||||
_kx(21, 0) =
|
||||
_tmp44 * (P(21, 0) * _tmp3 - P(21, 1) * _tmp7 + P(21, 2) * _tmp5 - P(21, 3) * _tmp1);
|
||||
_tmp30 * (P(21, 0) * _tmp5 - P(21, 1) * _tmp8 + P(21, 2) * _tmp7 - P(21, 3) * _tmp3);
|
||||
_kx(22, 0) =
|
||||
_tmp44 * (P(22, 0) * _tmp3 - P(22, 1) * _tmp7 + P(22, 2) * _tmp5 - P(22, 3) * _tmp1);
|
||||
_tmp30 * (P(22, 0) * _tmp5 - P(22, 1) * _tmp8 + P(22, 2) * _tmp7 - P(22, 3) * _tmp3);
|
||||
_kx(23, 0) =
|
||||
_tmp44 * (P(23, 0) * _tmp3 - P(23, 1) * _tmp7 + P(23, 2) * _tmp5 - P(23, 3) * _tmp1);
|
||||
_tmp30 * (P(23, 0) * _tmp5 - P(23, 1) * _tmp8 + P(23, 2) * _tmp7 - P(23, 3) * _tmp3);
|
||||
}
|
||||
|
||||
if (Ky != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _ky = (*Ky);
|
||||
|
||||
_ky(0, 0) = _tmp51 * (-P(0, 2) * _tmp7 - _tmp15 + _tmp19 + _tmp38);
|
||||
_ky(1, 0) = _tmp51 * (-P(1, 3) * _tmp3 + _tmp13 + _tmp26 - _tmp33);
|
||||
_ky(2, 0) = _tmp51 * (-P(2, 0) * _tmp1 + _tmp22 - _tmp41 - _tmp9);
|
||||
_ky(3, 0) = _tmp51 * (-P(3, 1) * _tmp5 + _tmp2 + _tmp27 + _tmp34);
|
||||
_ky(4, 0) =
|
||||
_tmp51 * (-P(4, 1) * _tmp5 - P(4, 2) * _tmp7 - P(4, 3) * _tmp3 - _tmp52 * state(1, 0));
|
||||
_ky(5, 0) = _tmp51 * (-P(5, 0) * _tmp1 - P(5, 1) * _tmp5 - P(5, 2) * _tmp7 - P(5, 3) * _tmp3);
|
||||
_ky(6, 0) = _tmp51 * (-P(6, 0) * _tmp1 - P(6, 1) * _tmp5 - P(6, 2) * _tmp7 - P(6, 3) * _tmp3);
|
||||
_ky(7, 0) = _tmp51 * (-P(7, 0) * _tmp1 - P(7, 1) * _tmp5 - P(7, 2) * _tmp7 - P(7, 3) * _tmp3);
|
||||
_ky(8, 0) =
|
||||
_tmp51 * (-P(8, 1) * _tmp5 - P(8, 2) * _tmp7 - _tmp45 * state(2, 0) - _tmp46 * state(1, 0));
|
||||
_ky(9, 0) =
|
||||
_tmp51 * (-P(9, 1) * _tmp5 - P(9, 2) * _tmp7 - _tmp48 * state(2, 0) - _tmp50 * state(1, 0));
|
||||
_ky(0, 0) = _tmp32 * (-P(0, 1) * _tmp7 - P(0, 2) * _tmp8 - _tmp14 + _tmp17);
|
||||
_ky(1, 0) = _tmp32 * (-P(1, 0) * _tmp3 - P(1, 3) * _tmp5 + _tmp12 + _tmp21);
|
||||
_ky(2, 0) = _tmp32 * (-P(2, 0) * _tmp3 - P(2, 3) * _tmp5 + _tmp18 - _tmp9);
|
||||
_ky(3, 0) = _tmp32 * (-P(3, 1) * _tmp7 - P(3, 2) * _tmp8 + _tmp22 + _tmp4);
|
||||
_ky(4, 0) = _tmp32 * (-P(4, 0) * _tmp3 - P(4, 1) * _tmp7 - P(4, 2) * _tmp8 - P(4, 3) * _tmp5);
|
||||
_ky(5, 0) = _tmp32 * (-P(5, 0) * _tmp3 - P(5, 1) * _tmp7 - P(5, 2) * _tmp8 - P(5, 3) * _tmp5);
|
||||
_ky(6, 0) =
|
||||
_tmp32 * (-P(6, 0) * _tmp3 - P(6, 1) * _tmp7 - P(6, 2) * _tmp8 - _tmp31 * state(2, 0));
|
||||
_ky(7, 0) = _tmp32 * (-P(7, 0) * _tmp3 - P(7, 1) * _tmp7 - P(7, 2) * _tmp8 - P(7, 3) * _tmp5);
|
||||
_ky(8, 0) = _tmp32 * (-P(8, 0) * _tmp3 - P(8, 1) * _tmp7 - P(8, 2) * _tmp8 - P(8, 3) * _tmp5);
|
||||
_ky(9, 0) = _tmp32 * (-P(9, 0) * _tmp3 - P(9, 1) * _tmp7 - P(9, 2) * _tmp8 - P(9, 3) * _tmp5);
|
||||
_ky(10, 0) =
|
||||
_tmp51 * (-P(10, 0) * _tmp1 - P(10, 1) * _tmp5 - P(10, 2) * _tmp7 - P(10, 3) * _tmp3);
|
||||
_tmp32 * (-P(10, 0) * _tmp3 - P(10, 1) * _tmp7 - P(10, 2) * _tmp8 - P(10, 3) * _tmp5);
|
||||
_ky(11, 0) =
|
||||
_tmp51 * (-P(11, 0) * _tmp1 - P(11, 1) * _tmp5 - P(11, 2) * _tmp7 - P(11, 3) * _tmp3);
|
||||
_tmp32 * (-P(11, 0) * _tmp3 - P(11, 1) * _tmp7 - P(11, 2) * _tmp8 - P(11, 3) * _tmp5);
|
||||
_ky(12, 0) =
|
||||
_tmp51 * (-P(12, 0) * _tmp1 - P(12, 1) * _tmp5 - P(12, 2) * _tmp7 - P(12, 3) * _tmp3);
|
||||
_tmp32 * (-P(12, 0) * _tmp3 - P(12, 1) * _tmp7 - P(12, 2) * _tmp8 - P(12, 3) * _tmp5);
|
||||
_ky(13, 0) =
|
||||
_tmp51 * (-P(13, 0) * _tmp1 - P(13, 1) * _tmp5 - P(13, 2) * _tmp7 - P(13, 3) * _tmp3);
|
||||
_tmp32 * (-P(13, 0) * _tmp3 - P(13, 1) * _tmp7 - P(13, 2) * _tmp8 - P(13, 3) * _tmp5);
|
||||
_ky(14, 0) =
|
||||
_tmp51 * (-P(14, 0) * _tmp1 - P(14, 1) * _tmp5 - P(14, 2) * _tmp7 - P(14, 3) * _tmp3);
|
||||
_tmp32 * (-P(14, 0) * _tmp3 - P(14, 1) * _tmp7 - P(14, 2) * _tmp8 - P(14, 3) * _tmp5);
|
||||
_ky(15, 0) =
|
||||
_tmp51 * (-P(15, 0) * _tmp1 - P(15, 1) * _tmp5 - P(15, 2) * _tmp7 - P(15, 3) * _tmp3);
|
||||
_tmp32 * (-P(15, 0) * _tmp3 - P(15, 1) * _tmp7 - P(15, 2) * _tmp8 - P(15, 3) * _tmp5);
|
||||
_ky(16, 0) =
|
||||
_tmp51 * (-P(16, 0) * _tmp1 - P(16, 1) * _tmp5 - P(16, 2) * _tmp7 - P(16, 3) * _tmp3);
|
||||
_tmp32 * (-P(16, 0) * _tmp3 - P(16, 1) * _tmp7 - P(16, 2) * _tmp8 - P(16, 3) * _tmp5);
|
||||
_ky(17, 0) =
|
||||
_tmp51 * (-P(17, 0) * _tmp1 - P(17, 1) * _tmp5 - P(17, 2) * _tmp7 - P(17, 3) * _tmp3);
|
||||
_tmp32 * (-P(17, 0) * _tmp3 - P(17, 1) * _tmp7 - P(17, 2) * _tmp8 - P(17, 3) * _tmp5);
|
||||
_ky(18, 0) =
|
||||
_tmp51 * (-P(18, 0) * _tmp1 - P(18, 1) * _tmp5 - P(18, 2) * _tmp7 - P(18, 3) * _tmp3);
|
||||
_tmp32 * (-P(18, 0) * _tmp3 - P(18, 1) * _tmp7 - P(18, 2) * _tmp8 - P(18, 3) * _tmp5);
|
||||
_ky(19, 0) =
|
||||
_tmp51 * (-P(19, 0) * _tmp1 - P(19, 1) * _tmp5 - P(19, 2) * _tmp7 - P(19, 3) * _tmp3);
|
||||
_tmp32 * (-P(19, 0) * _tmp3 - P(19, 1) * _tmp7 - P(19, 2) * _tmp8 - P(19, 3) * _tmp5);
|
||||
_ky(20, 0) =
|
||||
_tmp51 * (-P(20, 0) * _tmp1 - P(20, 1) * _tmp5 - P(20, 2) * _tmp7 - P(20, 3) * _tmp3);
|
||||
_tmp32 * (-P(20, 0) * _tmp3 - P(20, 1) * _tmp7 - P(20, 2) * _tmp8 - P(20, 3) * _tmp5);
|
||||
_ky(21, 0) =
|
||||
_tmp51 * (-P(21, 0) * _tmp1 - P(21, 1) * _tmp5 - P(21, 2) * _tmp7 - P(21, 3) * _tmp3);
|
||||
_tmp32 * (-P(21, 0) * _tmp3 - P(21, 1) * _tmp7 - P(21, 2) * _tmp8 - P(21, 3) * _tmp5);
|
||||
_ky(22, 0) =
|
||||
_tmp51 * (-P(22, 0) * _tmp1 - P(22, 1) * _tmp5 - P(22, 2) * _tmp7 - P(22, 3) * _tmp3);
|
||||
_tmp32 * (-P(22, 0) * _tmp3 - P(22, 1) * _tmp7 - P(22, 2) * _tmp8 - P(22, 3) * _tmp5);
|
||||
_ky(23, 0) =
|
||||
_tmp51 * (-P(23, 0) * _tmp1 - P(23, 1) * _tmp5 - P(23, 2) * _tmp7 - P(23, 3) * _tmp3);
|
||||
_tmp32 * (-P(23, 0) * _tmp3 - P(23, 1) * _tmp7 - P(23, 2) * _tmp8 - P(23, 3) * _tmp5);
|
||||
}
|
||||
|
||||
if (Kz != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _kz = (*Kz);
|
||||
|
||||
_kz(0, 0) = _tmp53 * (-P(0, 3) * _tmp7 + _tmp11 + _tmp24 + _tmp31);
|
||||
_kz(1, 0) = _tmp53 * (P(1, 2) * _tmp3 + _tmp16 + _tmp20 + _tmp39);
|
||||
_kz(2, 0) = _tmp53 * (P(2, 1) * _tmp1 + _tmp29 + _tmp36 - _tmp6);
|
||||
_kz(3, 0) = _tmp53 * (-P(3, 0) * _tmp5 + _tmp21 + _tmp40 + _tmp8);
|
||||
_kz(4, 0) =
|
||||
_tmp53 * (P(4, 1) * _tmp1 + P(4, 2) * _tmp3 - P(4, 3) * _tmp7 - _tmp52 * state(0, 0));
|
||||
_kz(5, 0) = _tmp53 * (-P(5, 0) * _tmp5 + P(5, 1) * _tmp1 + P(5, 2) * _tmp3 - P(5, 3) * _tmp7);
|
||||
_kz(6, 0) = _tmp53 * (-P(6, 0) * _tmp5 + P(6, 1) * _tmp1 + P(6, 2) * _tmp3 - P(6, 3) * _tmp7);
|
||||
_kz(7, 0) = _tmp53 * (-P(7, 0) * _tmp5 + P(7, 1) * _tmp1 + P(7, 2) * _tmp3 - P(7, 3) * _tmp7);
|
||||
_kz(8, 0) =
|
||||
_tmp53 * (P(8, 1) * _tmp1 - P(8, 3) * _tmp7 - _tmp46 * state(0, 0) + _tmp47 * state(2, 0));
|
||||
_kz(9, 0) =
|
||||
_tmp53 * (P(9, 1) * _tmp1 - P(9, 3) * _tmp7 + _tmp49 * state(2, 0) - _tmp50 * state(0, 0));
|
||||
_kz(10, 0) =
|
||||
_tmp53 * (-P(10, 0) * _tmp5 + P(10, 1) * _tmp1 + P(10, 2) * _tmp3 - P(10, 3) * _tmp7);
|
||||
_kz(11, 0) =
|
||||
_tmp53 * (-P(11, 0) * _tmp5 + P(11, 1) * _tmp1 + P(11, 2) * _tmp3 - P(11, 3) * _tmp7);
|
||||
_kz(12, 0) =
|
||||
_tmp53 * (-P(12, 0) * _tmp5 + P(12, 1) * _tmp1 + P(12, 2) * _tmp3 - P(12, 3) * _tmp7);
|
||||
_kz(13, 0) =
|
||||
_tmp53 * (-P(13, 0) * _tmp5 + P(13, 1) * _tmp1 + P(13, 2) * _tmp3 - P(13, 3) * _tmp7);
|
||||
_kz(14, 0) =
|
||||
_tmp53 * (-P(14, 0) * _tmp5 + P(14, 1) * _tmp1 + P(14, 2) * _tmp3 - P(14, 3) * _tmp7);
|
||||
_kz(15, 0) =
|
||||
_tmp53 * (-P(15, 0) * _tmp5 + P(15, 1) * _tmp1 + P(15, 2) * _tmp3 - P(15, 3) * _tmp7);
|
||||
_kz(16, 0) =
|
||||
_tmp53 * (-P(16, 0) * _tmp5 + P(16, 1) * _tmp1 + P(16, 2) * _tmp3 - P(16, 3) * _tmp7);
|
||||
_kz(17, 0) =
|
||||
_tmp53 * (-P(17, 0) * _tmp5 + P(17, 1) * _tmp1 + P(17, 2) * _tmp3 - P(17, 3) * _tmp7);
|
||||
_kz(18, 0) =
|
||||
_tmp53 * (-P(18, 0) * _tmp5 + P(18, 1) * _tmp1 + P(18, 2) * _tmp3 - P(18, 3) * _tmp7);
|
||||
_kz(19, 0) =
|
||||
_tmp53 * (-P(19, 0) * _tmp5 + P(19, 1) * _tmp1 + P(19, 2) * _tmp3 - P(19, 3) * _tmp7);
|
||||
_kz(20, 0) =
|
||||
_tmp53 * (-P(20, 0) * _tmp5 + P(20, 1) * _tmp1 + P(20, 2) * _tmp3 - P(20, 3) * _tmp7);
|
||||
_kz(21, 0) =
|
||||
_tmp53 * (-P(21, 0) * _tmp5 + P(21, 1) * _tmp1 + P(21, 2) * _tmp3 - P(21, 3) * _tmp7);
|
||||
_kz(22, 0) =
|
||||
_tmp53 * (-P(22, 0) * _tmp5 + P(22, 1) * _tmp1 + P(22, 2) * _tmp3 - P(22, 3) * _tmp7);
|
||||
_kz(23, 0) =
|
||||
_tmp53 * (-P(23, 0) * _tmp5 + P(23, 1) * _tmp1 + P(23, 2) * _tmp3 - P(23, 3) * _tmp7);
|
||||
_kz(0, 0) = _tmp33 * (P(0, 1) * _tmp27 + P(0, 2) * _tmp25);
|
||||
_kz(1, 0) = _tmp33 * (P(1, 2) * _tmp25 + _tmp28);
|
||||
_kz(2, 0) = _tmp33 * (P(2, 1) * _tmp27 + _tmp26);
|
||||
_kz(3, 0) = _tmp33 * (P(3, 1) * _tmp27 + P(3, 2) * _tmp25);
|
||||
_kz(4, 0) = _tmp33 * (P(4, 1) * _tmp27 + P(4, 2) * _tmp25);
|
||||
_kz(5, 0) = _tmp33 * (P(5, 1) * _tmp27 + P(5, 2) * _tmp25);
|
||||
_kz(6, 0) = _tmp33 * (P(6, 1) * _tmp27 + P(6, 2) * _tmp25);
|
||||
_kz(7, 0) = _tmp33 * (P(7, 1) * _tmp27 + P(7, 2) * _tmp25);
|
||||
_kz(8, 0) = _tmp33 * (P(8, 1) * _tmp27 + P(8, 2) * _tmp25);
|
||||
_kz(9, 0) = _tmp33 * (P(9, 1) * _tmp27 + P(9, 2) * _tmp25);
|
||||
_kz(10, 0) = _tmp33 * (P(10, 1) * _tmp27 + P(10, 2) * _tmp25);
|
||||
_kz(11, 0) = _tmp33 * (P(11, 1) * _tmp27 + P(11, 2) * _tmp25);
|
||||
_kz(12, 0) = _tmp33 * (P(12, 1) * _tmp27 + P(12, 2) * _tmp25);
|
||||
_kz(13, 0) = _tmp33 * (P(13, 1) * _tmp27 + P(13, 2) * _tmp25);
|
||||
_kz(14, 0) = _tmp33 * (P(14, 1) * _tmp27 + P(14, 2) * _tmp25);
|
||||
_kz(15, 0) = _tmp33 * (P(15, 1) * _tmp27 + P(15, 2) * _tmp25);
|
||||
_kz(16, 0) = _tmp33 * (P(16, 1) * _tmp27 + P(16, 2) * _tmp25);
|
||||
_kz(17, 0) = _tmp33 * (P(17, 1) * _tmp27 + P(17, 2) * _tmp25);
|
||||
_kz(18, 0) = _tmp33 * (P(18, 1) * _tmp27 + P(18, 2) * _tmp25);
|
||||
_kz(19, 0) = _tmp33 * (P(19, 1) * _tmp27 + P(19, 2) * _tmp25);
|
||||
_kz(20, 0) = _tmp33 * (P(20, 1) * _tmp27 + P(20, 2) * _tmp25);
|
||||
_kz(21, 0) = _tmp33 * (P(21, 1) * _tmp27 + P(21, 2) * _tmp25);
|
||||
_kz(22, 0) = _tmp33 * (P(22, 1) * _tmp27 + P(22, 2) * _tmp25);
|
||||
_kz(23, 0) = _tmp33 * (P(23, 1) * _tmp27 + P(23, 2) * _tmp25);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+108
-107
@@ -35,69 +35,70 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov = nullptr,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const Hx = nullptr) {
|
||||
// Total ops: 470
|
||||
// Total ops: 471
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (48)
|
||||
const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp1 = -_tmp0;
|
||||
const Scalar _tmp2 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp3 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp4 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp5 = -_tmp3 + _tmp4;
|
||||
const Scalar _tmp6 = _tmp1 + _tmp2 + _tmp5;
|
||||
const Scalar _tmp7 = state(0, 0) * state(3, 0);
|
||||
const Scalar _tmp8 = state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp9 = 2 * state(17, 0);
|
||||
const Scalar _tmp10 = state(0, 0) * state(2, 0);
|
||||
const Scalar _tmp11 = state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp12 = 2 * state(18, 0);
|
||||
const Scalar _tmp13 = -_tmp2;
|
||||
const Scalar _tmp14 = _tmp0 + _tmp13 + _tmp5;
|
||||
const Scalar _tmp15 = state(2, 0) * state(3, 0);
|
||||
const Scalar _tmp16 = state(0, 0) * state(1, 0);
|
||||
const Scalar _tmp17 = 2 * state(16, 0);
|
||||
const Scalar _tmp18 = _tmp1 + _tmp13 + _tmp3 + _tmp4;
|
||||
const Scalar _tmp19 = _tmp9 * state(3, 0);
|
||||
const Scalar _tmp20 = _tmp12 * state(2, 0);
|
||||
const Scalar _tmp21 = 2 * state(0, 0);
|
||||
const Scalar _tmp22 = _tmp21 * state(16, 0);
|
||||
const Scalar _tmp23 = _tmp19 - _tmp20 + _tmp22;
|
||||
const Scalar _tmp24 = _tmp12 * state(3, 0) + _tmp17 * state(1, 0) + _tmp9 * state(2, 0);
|
||||
const Scalar _tmp25 = _tmp17 * state(3, 0);
|
||||
const Scalar _tmp26 = _tmp12 * state(1, 0);
|
||||
const Scalar _tmp27 = _tmp21 * state(17, 0);
|
||||
const Scalar _tmp28 = -_tmp25 + _tmp26 + _tmp27;
|
||||
const Scalar _tmp29 = _tmp17 * state(2, 0);
|
||||
const Scalar _tmp30 = _tmp9 * state(1, 0);
|
||||
const Scalar _tmp31 = _tmp12 * state(0, 0);
|
||||
const Scalar _tmp32 = -_tmp29 + _tmp30 - _tmp31;
|
||||
const Scalar _tmp33 = 2 * _tmp7;
|
||||
const Scalar _tmp34 = 2 * _tmp8;
|
||||
// Intermediate terms (49)
|
||||
const Scalar _tmp0 = -2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = -2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp2 = _tmp0 + _tmp1 + 1;
|
||||
const Scalar _tmp3 = 2 * state(0, 0);
|
||||
const Scalar _tmp4 = _tmp3 * state(3, 0);
|
||||
const Scalar _tmp5 = 2 * state(2, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(1, 0);
|
||||
const Scalar _tmp7 = _tmp4 + _tmp6;
|
||||
const Scalar _tmp8 = _tmp5 * state(0, 0);
|
||||
const Scalar _tmp9 = 2 * state(1, 0);
|
||||
const Scalar _tmp10 = _tmp9 * state(3, 0);
|
||||
const Scalar _tmp11 = _tmp10 - _tmp8;
|
||||
const Scalar _tmp12 = 1 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp13 = _tmp0 + _tmp12;
|
||||
const Scalar _tmp14 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp15 = _tmp3 * state(1, 0);
|
||||
const Scalar _tmp16 = _tmp14 + _tmp15;
|
||||
const Scalar _tmp17 = -_tmp4 + _tmp6;
|
||||
const Scalar _tmp18 = _tmp1 + _tmp12;
|
||||
const Scalar _tmp19 = _tmp14 - _tmp15;
|
||||
const Scalar _tmp20 = _tmp10 + _tmp8;
|
||||
const Scalar _tmp21 = _tmp9 * state(18, 0);
|
||||
const Scalar _tmp22 = _tmp3 * state(17, 0);
|
||||
const Scalar _tmp23 = _tmp21 + _tmp22 - 4 * state(16, 0) * state(3, 0);
|
||||
const Scalar _tmp24 = 4 * state(2, 0);
|
||||
const Scalar _tmp25 = _tmp9 * state(17, 0);
|
||||
const Scalar _tmp26 = _tmp3 * state(18, 0);
|
||||
const Scalar _tmp27 = -_tmp24 * state(16, 0) + _tmp25 - _tmp26;
|
||||
const Scalar _tmp28 = state(17, 0) * state(3, 0);
|
||||
const Scalar _tmp29 = 2 * _tmp28;
|
||||
const Scalar _tmp30 = _tmp5 * state(18, 0);
|
||||
const Scalar _tmp31 = _tmp29 - _tmp30;
|
||||
const Scalar _tmp32 = 2 * state(3, 0);
|
||||
const Scalar _tmp33 = _tmp32 * state(18, 0);
|
||||
const Scalar _tmp34 = _tmp5 * state(17, 0);
|
||||
const Scalar _tmp35 = _tmp33 + _tmp34;
|
||||
const Scalar _tmp36 = 2 * _tmp10;
|
||||
const Scalar _tmp37 = 2 * _tmp11;
|
||||
const Scalar _tmp38 = -_tmp36 + _tmp37;
|
||||
const Scalar _tmp39 = _tmp29 - _tmp30 + _tmp31;
|
||||
const Scalar _tmp40 = -_tmp19 + _tmp20 - _tmp22;
|
||||
const Scalar _tmp41 = -_tmp33 + _tmp34;
|
||||
const Scalar _tmp42 = 2 * _tmp15;
|
||||
const Scalar _tmp43 = 2 * _tmp16;
|
||||
const Scalar _tmp44 = _tmp42 + _tmp43;
|
||||
const Scalar _tmp45 = _tmp25 - _tmp26 - _tmp27;
|
||||
const Scalar _tmp46 = _tmp36 + _tmp37;
|
||||
const Scalar _tmp47 = _tmp42 - _tmp43;
|
||||
const Scalar _tmp36 = _tmp5 * state(16, 0);
|
||||
const Scalar _tmp37 = 4 * state(1, 0);
|
||||
const Scalar _tmp38 = _tmp26 + _tmp36 - _tmp37 * state(17, 0);
|
||||
const Scalar _tmp39 = _tmp3 * state(16, 0);
|
||||
const Scalar _tmp40 = -4 * _tmp28 + _tmp30 - _tmp39;
|
||||
const Scalar _tmp41 = _tmp32 * state(16, 0);
|
||||
const Scalar _tmp42 = _tmp21 - _tmp41;
|
||||
const Scalar _tmp43 = _tmp9 * state(16, 0);
|
||||
const Scalar _tmp44 = _tmp33 + _tmp43;
|
||||
const Scalar _tmp45 = -_tmp22 - _tmp37 * state(18, 0) + _tmp41;
|
||||
const Scalar _tmp46 = -_tmp24 * state(18, 0) + _tmp29 + _tmp39;
|
||||
const Scalar _tmp47 = _tmp34 + _tmp43;
|
||||
const Scalar _tmp48 = -_tmp25 + _tmp36;
|
||||
|
||||
// Output terms (3)
|
||||
if (innov != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _innov = (*innov);
|
||||
|
||||
_innov(0, 0) = _tmp12 * (-_tmp10 + _tmp11) + _tmp6 * state(16, 0) + _tmp9 * (_tmp7 + _tmp8) -
|
||||
_innov(0, 0) = _tmp11 * state(18, 0) + _tmp2 * state(16, 0) + _tmp7 * state(17, 0) -
|
||||
meas(0, 0) + state(19, 0);
|
||||
_innov(1, 0) = _tmp12 * (_tmp15 + _tmp16) + _tmp14 * state(17, 0) + _tmp17 * (-_tmp7 + _tmp8) -
|
||||
_innov(1, 0) = _tmp13 * state(17, 0) + _tmp16 * state(18, 0) + _tmp17 * state(16, 0) -
|
||||
meas(1, 0) + state(20, 0);
|
||||
_innov(2, 0) = _tmp17 * (_tmp10 + _tmp11) + _tmp18 * state(18, 0) + _tmp9 * (_tmp15 - _tmp16) -
|
||||
_innov(2, 0) = _tmp18 * state(18, 0) + _tmp19 * state(17, 0) + _tmp20 * state(16, 0) -
|
||||
meas(2, 0) + state(21, 0);
|
||||
}
|
||||
|
||||
@@ -105,56 +106,56 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
matrix::Matrix<Scalar, 3, 1>& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var(0, 0) =
|
||||
P(0, 19) * _tmp23 + P(1, 19) * _tmp24 + P(16, 19) * _tmp6 + P(17, 19) * _tmp35 +
|
||||
P(18, 19) * _tmp38 + P(19, 19) + P(2, 19) * _tmp32 + P(3, 19) * _tmp28 + R +
|
||||
_tmp23 * (P(0, 0) * _tmp23 + P(1, 0) * _tmp24 + P(16, 0) * _tmp6 + P(17, 0) * _tmp35 +
|
||||
P(18, 0) * _tmp38 + P(19, 0) + P(2, 0) * _tmp32 + P(3, 0) * _tmp28) +
|
||||
_tmp24 * (P(0, 1) * _tmp23 + P(1, 1) * _tmp24 + P(16, 1) * _tmp6 + P(17, 1) * _tmp35 +
|
||||
P(18, 1) * _tmp38 + P(19, 1) + P(2, 1) * _tmp32 + P(3, 1) * _tmp28) +
|
||||
_tmp28 * (P(0, 3) * _tmp23 + P(1, 3) * _tmp24 + P(16, 3) * _tmp6 + P(17, 3) * _tmp35 +
|
||||
P(18, 3) * _tmp38 + P(19, 3) + P(2, 3) * _tmp32 + P(3, 3) * _tmp28) +
|
||||
_tmp32 * (P(0, 2) * _tmp23 + P(1, 2) * _tmp24 + P(16, 2) * _tmp6 + P(17, 2) * _tmp35 +
|
||||
P(18, 2) * _tmp38 + P(19, 2) + P(2, 2) * _tmp32 + P(3, 2) * _tmp28) +
|
||||
_tmp35 * (P(0, 17) * _tmp23 + P(1, 17) * _tmp24 + P(16, 17) * _tmp6 + P(17, 17) * _tmp35 +
|
||||
P(18, 17) * _tmp38 + P(19, 17) + P(2, 17) * _tmp32 + P(3, 17) * _tmp28) +
|
||||
_tmp38 * (P(0, 18) * _tmp23 + P(1, 18) * _tmp24 + P(16, 18) * _tmp6 + P(17, 18) * _tmp35 +
|
||||
P(18, 18) * _tmp38 + P(19, 18) + P(2, 18) * _tmp32 + P(3, 18) * _tmp28) +
|
||||
_tmp6 * (P(0, 16) * _tmp23 + P(1, 16) * _tmp24 + P(16, 16) * _tmp6 + P(17, 16) * _tmp35 +
|
||||
P(18, 16) * _tmp38 + P(19, 16) + P(2, 16) * _tmp32 + P(3, 16) * _tmp28);
|
||||
P(0, 19) * _tmp31 + P(1, 19) * _tmp35 + P(16, 19) * _tmp2 + P(17, 19) * _tmp7 +
|
||||
P(18, 19) * _tmp11 + P(19, 19) + P(2, 19) * _tmp27 + P(3, 19) * _tmp23 + R +
|
||||
_tmp11 * (P(0, 18) * _tmp31 + P(1, 18) * _tmp35 + P(16, 18) * _tmp2 + P(17, 18) * _tmp7 +
|
||||
P(18, 18) * _tmp11 + P(19, 18) + P(2, 18) * _tmp27 + P(3, 18) * _tmp23) +
|
||||
_tmp2 * (P(0, 16) * _tmp31 + P(1, 16) * _tmp35 + P(16, 16) * _tmp2 + P(17, 16) * _tmp7 +
|
||||
P(18, 16) * _tmp11 + P(19, 16) + P(2, 16) * _tmp27 + P(3, 16) * _tmp23) +
|
||||
_tmp23 * (P(0, 3) * _tmp31 + P(1, 3) * _tmp35 + P(16, 3) * _tmp2 + P(17, 3) * _tmp7 +
|
||||
P(18, 3) * _tmp11 + P(19, 3) + P(2, 3) * _tmp27 + P(3, 3) * _tmp23) +
|
||||
_tmp27 * (P(0, 2) * _tmp31 + P(1, 2) * _tmp35 + P(16, 2) * _tmp2 + P(17, 2) * _tmp7 +
|
||||
P(18, 2) * _tmp11 + P(19, 2) + P(2, 2) * _tmp27 + P(3, 2) * _tmp23) +
|
||||
_tmp31 * (P(0, 0) * _tmp31 + P(1, 0) * _tmp35 + P(16, 0) * _tmp2 + P(17, 0) * _tmp7 +
|
||||
P(18, 0) * _tmp11 + P(19, 0) + P(2, 0) * _tmp27 + P(3, 0) * _tmp23) +
|
||||
_tmp35 * (P(0, 1) * _tmp31 + P(1, 1) * _tmp35 + P(16, 1) * _tmp2 + P(17, 1) * _tmp7 +
|
||||
P(18, 1) * _tmp11 + P(19, 1) + P(2, 1) * _tmp27 + P(3, 1) * _tmp23) +
|
||||
_tmp7 * (P(0, 17) * _tmp31 + P(1, 17) * _tmp35 + P(16, 17) * _tmp2 + P(17, 17) * _tmp7 +
|
||||
P(18, 17) * _tmp11 + P(19, 17) + P(2, 17) * _tmp27 + P(3, 17) * _tmp23);
|
||||
_innov_var(1, 0) =
|
||||
P(0, 20) * _tmp28 + P(1, 20) * _tmp39 + P(16, 20) * _tmp41 + P(17, 20) * _tmp14 +
|
||||
P(18, 20) * _tmp44 + P(2, 20) * _tmp24 + P(20, 20) + P(3, 20) * _tmp40 + R +
|
||||
_tmp14 * (P(0, 17) * _tmp28 + P(1, 17) * _tmp39 + P(16, 17) * _tmp41 + P(17, 17) * _tmp14 +
|
||||
P(18, 17) * _tmp44 + P(2, 17) * _tmp24 + P(20, 17) + P(3, 17) * _tmp40) +
|
||||
_tmp24 * (P(0, 2) * _tmp28 + P(1, 2) * _tmp39 + P(16, 2) * _tmp41 + P(17, 2) * _tmp14 +
|
||||
P(18, 2) * _tmp44 + P(2, 2) * _tmp24 + P(20, 2) + P(3, 2) * _tmp40) +
|
||||
_tmp28 * (P(0, 0) * _tmp28 + P(1, 0) * _tmp39 + P(16, 0) * _tmp41 + P(17, 0) * _tmp14 +
|
||||
P(18, 0) * _tmp44 + P(2, 0) * _tmp24 + P(20, 0) + P(3, 0) * _tmp40) +
|
||||
_tmp39 * (P(0, 1) * _tmp28 + P(1, 1) * _tmp39 + P(16, 1) * _tmp41 + P(17, 1) * _tmp14 +
|
||||
P(18, 1) * _tmp44 + P(2, 1) * _tmp24 + P(20, 1) + P(3, 1) * _tmp40) +
|
||||
_tmp40 * (P(0, 3) * _tmp28 + P(1, 3) * _tmp39 + P(16, 3) * _tmp41 + P(17, 3) * _tmp14 +
|
||||
P(18, 3) * _tmp44 + P(2, 3) * _tmp24 + P(20, 3) + P(3, 3) * _tmp40) +
|
||||
_tmp41 * (P(0, 16) * _tmp28 + P(1, 16) * _tmp39 + P(16, 16) * _tmp41 + P(17, 16) * _tmp14 +
|
||||
P(18, 16) * _tmp44 + P(2, 16) * _tmp24 + P(20, 16) + P(3, 16) * _tmp40) +
|
||||
_tmp44 * (P(0, 18) * _tmp28 + P(1, 18) * _tmp39 + P(16, 18) * _tmp41 + P(17, 18) * _tmp14 +
|
||||
P(18, 18) * _tmp44 + P(2, 18) * _tmp24 + P(20, 18) + P(3, 18) * _tmp40);
|
||||
P(0, 20) * _tmp42 + P(1, 20) * _tmp38 + P(16, 20) * _tmp17 + P(17, 20) * _tmp13 +
|
||||
P(18, 20) * _tmp16 + P(2, 20) * _tmp44 + P(20, 20) + P(3, 20) * _tmp40 + R +
|
||||
_tmp13 * (P(0, 17) * _tmp42 + P(1, 17) * _tmp38 + P(16, 17) * _tmp17 + P(17, 17) * _tmp13 +
|
||||
P(18, 17) * _tmp16 + P(2, 17) * _tmp44 + P(20, 17) + P(3, 17) * _tmp40) +
|
||||
_tmp16 * (P(0, 18) * _tmp42 + P(1, 18) * _tmp38 + P(16, 18) * _tmp17 + P(17, 18) * _tmp13 +
|
||||
P(18, 18) * _tmp16 + P(2, 18) * _tmp44 + P(20, 18) + P(3, 18) * _tmp40) +
|
||||
_tmp17 * (P(0, 16) * _tmp42 + P(1, 16) * _tmp38 + P(16, 16) * _tmp17 + P(17, 16) * _tmp13 +
|
||||
P(18, 16) * _tmp16 + P(2, 16) * _tmp44 + P(20, 16) + P(3, 16) * _tmp40) +
|
||||
_tmp38 * (P(0, 1) * _tmp42 + P(1, 1) * _tmp38 + P(16, 1) * _tmp17 + P(17, 1) * _tmp13 +
|
||||
P(18, 1) * _tmp16 + P(2, 1) * _tmp44 + P(20, 1) + P(3, 1) * _tmp40) +
|
||||
_tmp40 * (P(0, 3) * _tmp42 + P(1, 3) * _tmp38 + P(16, 3) * _tmp17 + P(17, 3) * _tmp13 +
|
||||
P(18, 3) * _tmp16 + P(2, 3) * _tmp44 + P(20, 3) + P(3, 3) * _tmp40) +
|
||||
_tmp42 * (P(0, 0) * _tmp42 + P(1, 0) * _tmp38 + P(16, 0) * _tmp17 + P(17, 0) * _tmp13 +
|
||||
P(18, 0) * _tmp16 + P(2, 0) * _tmp44 + P(20, 0) + P(3, 0) * _tmp40) +
|
||||
_tmp44 * (P(0, 2) * _tmp42 + P(1, 2) * _tmp38 + P(16, 2) * _tmp17 + P(17, 2) * _tmp13 +
|
||||
P(18, 2) * _tmp16 + P(2, 2) * _tmp44 + P(20, 2) + P(3, 2) * _tmp40);
|
||||
_innov_var(2, 0) =
|
||||
P(0, 21) * _tmp39 + P(1, 21) * _tmp45 + P(16, 21) * _tmp46 + P(17, 21) * _tmp47 +
|
||||
P(18, 21) * _tmp18 + P(2, 21) * _tmp23 + P(21, 21) + P(3, 21) * _tmp24 + R +
|
||||
_tmp18 * (P(0, 18) * _tmp39 + P(1, 18) * _tmp45 + P(16, 18) * _tmp46 + P(17, 18) * _tmp47 +
|
||||
P(18, 18) * _tmp18 + P(2, 18) * _tmp23 + P(21, 18) + P(3, 18) * _tmp24) +
|
||||
_tmp23 * (P(0, 2) * _tmp39 + P(1, 2) * _tmp45 + P(16, 2) * _tmp46 + P(17, 2) * _tmp47 +
|
||||
P(18, 2) * _tmp18 + P(2, 2) * _tmp23 + P(21, 2) + P(3, 2) * _tmp24) +
|
||||
_tmp24 * (P(0, 3) * _tmp39 + P(1, 3) * _tmp45 + P(16, 3) * _tmp46 + P(17, 3) * _tmp47 +
|
||||
P(18, 3) * _tmp18 + P(2, 3) * _tmp23 + P(21, 3) + P(3, 3) * _tmp24) +
|
||||
_tmp39 * (P(0, 0) * _tmp39 + P(1, 0) * _tmp45 + P(16, 0) * _tmp46 + P(17, 0) * _tmp47 +
|
||||
P(18, 0) * _tmp18 + P(2, 0) * _tmp23 + P(21, 0) + P(3, 0) * _tmp24) +
|
||||
_tmp45 * (P(0, 1) * _tmp39 + P(1, 1) * _tmp45 + P(16, 1) * _tmp46 + P(17, 1) * _tmp47 +
|
||||
P(18, 1) * _tmp18 + P(2, 1) * _tmp23 + P(21, 1) + P(3, 1) * _tmp24) +
|
||||
_tmp46 * (P(0, 16) * _tmp39 + P(1, 16) * _tmp45 + P(16, 16) * _tmp46 + P(17, 16) * _tmp47 +
|
||||
P(18, 16) * _tmp18 + P(2, 16) * _tmp23 + P(21, 16) + P(3, 16) * _tmp24) +
|
||||
_tmp47 * (P(0, 17) * _tmp39 + P(1, 17) * _tmp45 + P(16, 17) * _tmp46 + P(17, 17) * _tmp47 +
|
||||
P(18, 17) * _tmp18 + P(2, 17) * _tmp23 + P(21, 17) + P(3, 17) * _tmp24);
|
||||
P(0, 21) * _tmp48 + P(1, 21) * _tmp45 + P(16, 21) * _tmp20 + P(17, 21) * _tmp19 +
|
||||
P(18, 21) * _tmp18 + P(2, 21) * _tmp46 + P(21, 21) + P(3, 21) * _tmp47 + R +
|
||||
_tmp18 * (P(0, 18) * _tmp48 + P(1, 18) * _tmp45 + P(16, 18) * _tmp20 + P(17, 18) * _tmp19 +
|
||||
P(18, 18) * _tmp18 + P(2, 18) * _tmp46 + P(21, 18) + P(3, 18) * _tmp47) +
|
||||
_tmp19 * (P(0, 17) * _tmp48 + P(1, 17) * _tmp45 + P(16, 17) * _tmp20 + P(17, 17) * _tmp19 +
|
||||
P(18, 17) * _tmp18 + P(2, 17) * _tmp46 + P(21, 17) + P(3, 17) * _tmp47) +
|
||||
_tmp20 * (P(0, 16) * _tmp48 + P(1, 16) * _tmp45 + P(16, 16) * _tmp20 + P(17, 16) * _tmp19 +
|
||||
P(18, 16) * _tmp18 + P(2, 16) * _tmp46 + P(21, 16) + P(3, 16) * _tmp47) +
|
||||
_tmp45 * (P(0, 1) * _tmp48 + P(1, 1) * _tmp45 + P(16, 1) * _tmp20 + P(17, 1) * _tmp19 +
|
||||
P(18, 1) * _tmp18 + P(2, 1) * _tmp46 + P(21, 1) + P(3, 1) * _tmp47) +
|
||||
_tmp46 * (P(0, 2) * _tmp48 + P(1, 2) * _tmp45 + P(16, 2) * _tmp20 + P(17, 2) * _tmp19 +
|
||||
P(18, 2) * _tmp18 + P(2, 2) * _tmp46 + P(21, 2) + P(3, 2) * _tmp47) +
|
||||
_tmp47 * (P(0, 3) * _tmp48 + P(1, 3) * _tmp45 + P(16, 3) * _tmp20 + P(17, 3) * _tmp19 +
|
||||
P(18, 3) * _tmp18 + P(2, 3) * _tmp46 + P(21, 3) + P(3, 3) * _tmp47) +
|
||||
_tmp48 * (P(0, 0) * _tmp48 + P(1, 0) * _tmp45 + P(16, 0) * _tmp20 + P(17, 0) * _tmp19 +
|
||||
P(18, 0) * _tmp18 + P(2, 0) * _tmp46 + P(21, 0) + P(3, 0) * _tmp47);
|
||||
}
|
||||
|
||||
if (Hx != nullptr) {
|
||||
@@ -162,13 +163,13 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_hx.setZero();
|
||||
|
||||
_hx(0, 0) = _tmp23;
|
||||
_hx(1, 0) = _tmp24;
|
||||
_hx(2, 0) = _tmp32;
|
||||
_hx(3, 0) = _tmp28;
|
||||
_hx(16, 0) = _tmp6;
|
||||
_hx(17, 0) = _tmp35;
|
||||
_hx(18, 0) = _tmp38;
|
||||
_hx(0, 0) = _tmp31;
|
||||
_hx(1, 0) = _tmp35;
|
||||
_hx(2, 0) = _tmp27;
|
||||
_hx(3, 0) = _tmp23;
|
||||
_hx(16, 0) = _tmp2;
|
||||
_hx(17, 0) = _tmp7;
|
||||
_hx(18, 0) = _tmp11;
|
||||
_hx(19, 0) = 1;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
+38
-37
@@ -30,45 +30,46 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 164
|
||||
// Total ops: 160
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (11)
|
||||
const Scalar _tmp0 = 2 * state(3, 0);
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 = 2 * state(0, 0);
|
||||
const Scalar _tmp3 = -_tmp0 * state(16, 0) + _tmp1 * state(18, 0) + _tmp2 * state(17, 0);
|
||||
const Scalar _tmp4 = 2 * state(2, 0);
|
||||
const Scalar _tmp5 = -_tmp1 * state(17, 0) + _tmp2 * state(18, 0) + _tmp4 * state(16, 0);
|
||||
const Scalar _tmp6 = -_tmp0 * state(17, 0) - _tmp2 * state(16, 0) + _tmp4 * state(18, 0);
|
||||
const Scalar _tmp7 = _tmp0 * state(18, 0) + _tmp1 * state(16, 0) + _tmp4 * state(17, 0);
|
||||
const Scalar _tmp8 = -_tmp0 * state(0, 0) + _tmp1 * state(2, 0);
|
||||
const Scalar _tmp9 = _tmp0 * state(2, 0) + _tmp1 * state(0, 0);
|
||||
const Scalar _tmp10 = 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));
|
||||
// Intermediate terms (12)
|
||||
const Scalar _tmp0 = 2 * state(16, 0);
|
||||
const Scalar _tmp1 = 4 * state(17, 0);
|
||||
const Scalar _tmp2 = 2 * state(18, 0);
|
||||
const Scalar _tmp3 = _tmp0 * state(2, 0) - _tmp1 * state(1, 0) + _tmp2 * state(0, 0);
|
||||
const Scalar _tmp4 = -_tmp0 * state(0, 0) - _tmp1 * state(3, 0) + _tmp2 * state(2, 0);
|
||||
const Scalar _tmp5 = -_tmp0 * state(3, 0) + _tmp2 * state(1, 0);
|
||||
const Scalar _tmp6 = _tmp0 * state(1, 0) + _tmp2 * state(3, 0);
|
||||
const Scalar _tmp7 =
|
||||
-2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
|
||||
const Scalar _tmp8 = 2 * state(3, 0);
|
||||
const Scalar _tmp9 = 2 * state(1, 0);
|
||||
const Scalar _tmp10 = _tmp8 * state(2, 0) + _tmp9 * state(0, 0);
|
||||
const Scalar _tmp11 = -_tmp8 * state(0, 0) + _tmp9 * state(2, 0);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var =
|
||||
P(0, 20) * _tmp3 + P(1, 20) * _tmp5 + P(16, 20) * _tmp8 + P(17, 20) * _tmp10 +
|
||||
P(18, 20) * _tmp9 + P(2, 20) * _tmp7 + P(20, 20) + P(3, 20) * _tmp6 + R +
|
||||
_tmp10 * (P(0, 17) * _tmp3 + P(1, 17) * _tmp5 + P(16, 17) * _tmp8 + P(17, 17) * _tmp10 +
|
||||
P(18, 17) * _tmp9 + P(2, 17) * _tmp7 + P(20, 17) + P(3, 17) * _tmp6) +
|
||||
_tmp3 * (P(0, 0) * _tmp3 + P(1, 0) * _tmp5 + P(16, 0) * _tmp8 + P(17, 0) * _tmp10 +
|
||||
P(18, 0) * _tmp9 + P(2, 0) * _tmp7 + P(20, 0) + P(3, 0) * _tmp6) +
|
||||
_tmp5 * (P(0, 1) * _tmp3 + P(1, 1) * _tmp5 + P(16, 1) * _tmp8 + P(17, 1) * _tmp10 +
|
||||
P(18, 1) * _tmp9 + P(2, 1) * _tmp7 + P(20, 1) + P(3, 1) * _tmp6) +
|
||||
_tmp6 * (P(0, 3) * _tmp3 + P(1, 3) * _tmp5 + P(16, 3) * _tmp8 + P(17, 3) * _tmp10 +
|
||||
P(18, 3) * _tmp9 + P(2, 3) * _tmp7 + P(20, 3) + P(3, 3) * _tmp6) +
|
||||
_tmp7 * (P(0, 2) * _tmp3 + P(1, 2) * _tmp5 + P(16, 2) * _tmp8 + P(17, 2) * _tmp10 +
|
||||
P(18, 2) * _tmp9 + P(2, 2) * _tmp7 + P(20, 2) + P(3, 2) * _tmp6) +
|
||||
_tmp8 * (P(0, 16) * _tmp3 + P(1, 16) * _tmp5 + P(16, 16) * _tmp8 + P(17, 16) * _tmp10 +
|
||||
P(18, 16) * _tmp9 + P(2, 16) * _tmp7 + P(20, 16) + P(3, 16) * _tmp6) +
|
||||
_tmp9 * (P(0, 18) * _tmp3 + P(1, 18) * _tmp5 + P(16, 18) * _tmp8 + P(17, 18) * _tmp10 +
|
||||
P(18, 18) * _tmp9 + P(2, 18) * _tmp7 + P(20, 18) + P(3, 18) * _tmp6);
|
||||
P(0, 20) * _tmp5 + P(1, 20) * _tmp3 + P(16, 20) * _tmp11 + P(17, 20) * _tmp7 +
|
||||
P(18, 20) * _tmp10 + P(2, 20) * _tmp6 + P(20, 20) + P(3, 20) * _tmp4 + R +
|
||||
_tmp10 * (P(0, 18) * _tmp5 + P(1, 18) * _tmp3 + P(16, 18) * _tmp11 + P(17, 18) * _tmp7 +
|
||||
P(18, 18) * _tmp10 + P(2, 18) * _tmp6 + P(20, 18) + P(3, 18) * _tmp4) +
|
||||
_tmp11 * (P(0, 16) * _tmp5 + P(1, 16) * _tmp3 + P(16, 16) * _tmp11 + P(17, 16) * _tmp7 +
|
||||
P(18, 16) * _tmp10 + P(2, 16) * _tmp6 + P(20, 16) + P(3, 16) * _tmp4) +
|
||||
_tmp3 * (P(0, 1) * _tmp5 + P(1, 1) * _tmp3 + P(16, 1) * _tmp11 + P(17, 1) * _tmp7 +
|
||||
P(18, 1) * _tmp10 + P(2, 1) * _tmp6 + P(20, 1) + P(3, 1) * _tmp4) +
|
||||
_tmp4 * (P(0, 3) * _tmp5 + P(1, 3) * _tmp3 + P(16, 3) * _tmp11 + P(17, 3) * _tmp7 +
|
||||
P(18, 3) * _tmp10 + P(2, 3) * _tmp6 + P(20, 3) + P(3, 3) * _tmp4) +
|
||||
_tmp5 * (P(0, 0) * _tmp5 + P(1, 0) * _tmp3 + P(16, 0) * _tmp11 + P(17, 0) * _tmp7 +
|
||||
P(18, 0) * _tmp10 + P(2, 0) * _tmp6 + P(20, 0) + P(3, 0) * _tmp4) +
|
||||
_tmp6 * (P(0, 2) * _tmp5 + P(1, 2) * _tmp3 + P(16, 2) * _tmp11 + P(17, 2) * _tmp7 +
|
||||
P(18, 2) * _tmp10 + P(2, 2) * _tmp6 + P(20, 2) + P(3, 2) * _tmp4) +
|
||||
_tmp7 * (P(0, 17) * _tmp5 + P(1, 17) * _tmp3 + P(16, 17) * _tmp11 + P(17, 17) * _tmp7 +
|
||||
P(18, 17) * _tmp10 + P(2, 17) * _tmp6 + P(20, 17) + P(3, 17) * _tmp4);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
@@ -76,13 +77,13 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp3;
|
||||
_h(1, 0) = _tmp5;
|
||||
_h(2, 0) = _tmp7;
|
||||
_h(3, 0) = _tmp6;
|
||||
_h(16, 0) = _tmp8;
|
||||
_h(17, 0) = _tmp10;
|
||||
_h(18, 0) = _tmp9;
|
||||
_h(0, 0) = _tmp5;
|
||||
_h(1, 0) = _tmp3;
|
||||
_h(2, 0) = _tmp6;
|
||||
_h(3, 0) = _tmp4;
|
||||
_h(16, 0) = _tmp11;
|
||||
_h(17, 0) = _tmp7;
|
||||
_h(18, 0) = _tmp10;
|
||||
_h(20, 0) = 1;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
+38
-37
@@ -30,45 +30,46 @@ void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 164
|
||||
// Total ops: 160
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (11)
|
||||
const Scalar _tmp0 = 2 * state(2, 0);
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 = 2 * state(0, 0);
|
||||
const Scalar _tmp3 = _tmp0 * state(16, 0) - _tmp1 * state(17, 0) + _tmp2 * state(18, 0);
|
||||
const Scalar _tmp4 = 2 * state(3, 0);
|
||||
const Scalar _tmp5 = -_tmp1 * state(18, 0) - _tmp2 * state(17, 0) + _tmp4 * state(16, 0);
|
||||
const Scalar _tmp6 = _tmp0 * state(17, 0) + _tmp1 * state(16, 0) + _tmp4 * state(18, 0);
|
||||
const Scalar _tmp7 = -_tmp0 * state(18, 0) + _tmp2 * state(16, 0) + _tmp4 * state(17, 0);
|
||||
const Scalar _tmp8 = _tmp0 * state(0, 0) + _tmp1 * state(3, 0);
|
||||
const Scalar _tmp9 = _tmp0 * state(3, 0) - _tmp1 * state(0, 0);
|
||||
const Scalar _tmp10 = 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));
|
||||
// Intermediate terms (12)
|
||||
const Scalar _tmp0 = 2 * state(16, 0);
|
||||
const Scalar _tmp1 = 4 * state(18, 0);
|
||||
const Scalar _tmp2 = 2 * state(17, 0);
|
||||
const Scalar _tmp3 = _tmp0 * state(3, 0) - _tmp1 * state(1, 0) - _tmp2 * state(0, 0);
|
||||
const Scalar _tmp4 = _tmp0 * state(0, 0) - _tmp1 * state(2, 0) + _tmp2 * state(3, 0);
|
||||
const Scalar _tmp5 = _tmp0 * state(1, 0) + _tmp2 * state(2, 0);
|
||||
const Scalar _tmp6 = _tmp0 * state(2, 0) - _tmp2 * state(1, 0);
|
||||
const Scalar _tmp7 = 2 * state(2, 0);
|
||||
const Scalar _tmp8 = 2 * state(1, 0);
|
||||
const Scalar _tmp9 = _tmp7 * state(0, 0) + _tmp8 * state(3, 0);
|
||||
const Scalar _tmp10 = _tmp7 * state(3, 0) - _tmp8 * state(0, 0);
|
||||
const Scalar _tmp11 =
|
||||
-2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1;
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var =
|
||||
P(0, 21) * _tmp3 + P(1, 21) * _tmp5 + P(16, 21) * _tmp8 + P(17, 21) * _tmp9 +
|
||||
P(18, 21) * _tmp10 + P(2, 21) * _tmp7 + P(21, 21) + P(3, 21) * _tmp6 + R +
|
||||
_tmp10 * (P(0, 18) * _tmp3 + P(1, 18) * _tmp5 + P(16, 18) * _tmp8 + P(17, 18) * _tmp9 +
|
||||
P(18, 18) * _tmp10 + P(2, 18) * _tmp7 + P(21, 18) + P(3, 18) * _tmp6) +
|
||||
_tmp3 * (P(0, 0) * _tmp3 + P(1, 0) * _tmp5 + P(16, 0) * _tmp8 + P(17, 0) * _tmp9 +
|
||||
P(18, 0) * _tmp10 + P(2, 0) * _tmp7 + P(21, 0) + P(3, 0) * _tmp6) +
|
||||
_tmp5 * (P(0, 1) * _tmp3 + P(1, 1) * _tmp5 + P(16, 1) * _tmp8 + P(17, 1) * _tmp9 +
|
||||
P(18, 1) * _tmp10 + P(2, 1) * _tmp7 + P(21, 1) + P(3, 1) * _tmp6) +
|
||||
_tmp6 * (P(0, 3) * _tmp3 + P(1, 3) * _tmp5 + P(16, 3) * _tmp8 + P(17, 3) * _tmp9 +
|
||||
P(18, 3) * _tmp10 + P(2, 3) * _tmp7 + P(21, 3) + P(3, 3) * _tmp6) +
|
||||
_tmp7 * (P(0, 2) * _tmp3 + P(1, 2) * _tmp5 + P(16, 2) * _tmp8 + P(17, 2) * _tmp9 +
|
||||
P(18, 2) * _tmp10 + P(2, 2) * _tmp7 + P(21, 2) + P(3, 2) * _tmp6) +
|
||||
_tmp8 * (P(0, 16) * _tmp3 + P(1, 16) * _tmp5 + P(16, 16) * _tmp8 + P(17, 16) * _tmp9 +
|
||||
P(18, 16) * _tmp10 + P(2, 16) * _tmp7 + P(21, 16) + P(3, 16) * _tmp6) +
|
||||
_tmp9 * (P(0, 17) * _tmp3 + P(1, 17) * _tmp5 + P(16, 17) * _tmp8 + P(17, 17) * _tmp9 +
|
||||
P(18, 17) * _tmp10 + P(2, 17) * _tmp7 + P(21, 17) + P(3, 17) * _tmp6);
|
||||
P(0, 21) * _tmp6 + P(1, 21) * _tmp3 + P(16, 21) * _tmp9 + P(17, 21) * _tmp10 +
|
||||
P(18, 21) * _tmp11 + P(2, 21) * _tmp4 + P(21, 21) + P(3, 21) * _tmp5 + R +
|
||||
_tmp10 * (P(0, 17) * _tmp6 + P(1, 17) * _tmp3 + P(16, 17) * _tmp9 + P(17, 17) * _tmp10 +
|
||||
P(18, 17) * _tmp11 + P(2, 17) * _tmp4 + P(21, 17) + P(3, 17) * _tmp5) +
|
||||
_tmp11 * (P(0, 18) * _tmp6 + P(1, 18) * _tmp3 + P(16, 18) * _tmp9 + P(17, 18) * _tmp10 +
|
||||
P(18, 18) * _tmp11 + P(2, 18) * _tmp4 + P(21, 18) + P(3, 18) * _tmp5) +
|
||||
_tmp3 * (P(0, 1) * _tmp6 + P(1, 1) * _tmp3 + P(16, 1) * _tmp9 + P(17, 1) * _tmp10 +
|
||||
P(18, 1) * _tmp11 + P(2, 1) * _tmp4 + P(21, 1) + P(3, 1) * _tmp5) +
|
||||
_tmp4 * (P(0, 2) * _tmp6 + P(1, 2) * _tmp3 + P(16, 2) * _tmp9 + P(17, 2) * _tmp10 +
|
||||
P(18, 2) * _tmp11 + P(2, 2) * _tmp4 + P(21, 2) + P(3, 2) * _tmp5) +
|
||||
_tmp5 * (P(0, 3) * _tmp6 + P(1, 3) * _tmp3 + P(16, 3) * _tmp9 + P(17, 3) * _tmp10 +
|
||||
P(18, 3) * _tmp11 + P(2, 3) * _tmp4 + P(21, 3) + P(3, 3) * _tmp5) +
|
||||
_tmp6 * (P(0, 0) * _tmp6 + P(1, 0) * _tmp3 + P(16, 0) * _tmp9 + P(17, 0) * _tmp10 +
|
||||
P(18, 0) * _tmp11 + P(2, 0) * _tmp4 + P(21, 0) + P(3, 0) * _tmp5) +
|
||||
_tmp9 * (P(0, 16) * _tmp6 + P(1, 16) * _tmp3 + P(16, 16) * _tmp9 + P(17, 16) * _tmp10 +
|
||||
P(18, 16) * _tmp11 + P(2, 16) * _tmp4 + P(21, 16) + P(3, 16) * _tmp5);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
@@ -76,13 +77,13 @@ void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp3;
|
||||
_h(1, 0) = _tmp5;
|
||||
_h(2, 0) = _tmp7;
|
||||
_h(3, 0) = _tmp6;
|
||||
_h(16, 0) = _tmp8;
|
||||
_h(17, 0) = _tmp9;
|
||||
_h(18, 0) = _tmp10;
|
||||
_h(0, 0) = _tmp6;
|
||||
_h(1, 0) = _tmp3;
|
||||
_h(2, 0) = _tmp4;
|
||||
_h(3, 0) = _tmp5;
|
||||
_h(16, 0) = _tmp9;
|
||||
_h(17, 0) = _tmp10;
|
||||
_h(18, 0) = _tmp11;
|
||||
_h(21, 0) = 1;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
@@ -30,57 +30,56 @@ void ComputeSideslipHAndK(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: 539
|
||||
// Total ops: 533
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (44)
|
||||
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 = -_tmp0 + _tmp1 + _tmp2;
|
||||
const Scalar _tmp4 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp5 = state(0, 0) * state(3, 0);
|
||||
const Scalar _tmp6 = state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp7 = _tmp5 + _tmp6;
|
||||
const Scalar _tmp8 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp9 = 2 * _tmp8;
|
||||
const Scalar _tmp10 = state(0, 0) * state(2, 0);
|
||||
const Scalar _tmp11 = state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp12 = 2 * state(6, 0);
|
||||
const Scalar _tmp13 = _tmp12 * (-_tmp10 + _tmp11) + _tmp3 * _tmp4 + _tmp7 * _tmp9;
|
||||
const Scalar _tmp14 =
|
||||
_tmp13 + epsilon * (2 * math::min<Scalar>(0, (((_tmp13) > 0) - ((_tmp13) < 0))) + 1);
|
||||
const Scalar _tmp15 = Scalar(1.0) / (_tmp14);
|
||||
const Scalar _tmp16 = 2 * _tmp4;
|
||||
const Scalar _tmp17 = _tmp12 * state(1, 0) - _tmp16 * state(3, 0) + _tmp9 * state(0, 0);
|
||||
const Scalar _tmp18 = _tmp16 * state(0, 0);
|
||||
const Scalar _tmp19 = _tmp9 * state(3, 0);
|
||||
const Scalar _tmp20 = _tmp12 * state(2, 0);
|
||||
const Scalar _tmp21 = _tmp0 - _tmp1 + _tmp2;
|
||||
const Scalar _tmp22 = -2 * _tmp5 + 2 * _tmp6;
|
||||
const Scalar _tmp23 = state(2, 0) * state(3, 0);
|
||||
const Scalar _tmp24 = state(0, 0) * state(1, 0);
|
||||
const Scalar _tmp25 =
|
||||
(_tmp12 * (_tmp23 + _tmp24) + _tmp21 * _tmp8 + _tmp22 * _tmp4) / std::pow(_tmp14, Scalar(2));
|
||||
const Scalar _tmp26 = _tmp15 * _tmp17 - _tmp25 * (_tmp18 + _tmp19 - _tmp20);
|
||||
const Scalar _tmp27 = _tmp9 * state(1, 0);
|
||||
const Scalar _tmp28 = _tmp16 * state(2, 0);
|
||||
const Scalar _tmp29 = _tmp12 * state(0, 0);
|
||||
const Scalar _tmp30 = _tmp12 * state(3, 0) + _tmp16 * state(1, 0) + _tmp9 * state(2, 0);
|
||||
const Scalar _tmp31 = _tmp15 * (-_tmp27 + _tmp28 + _tmp29) - _tmp25 * _tmp30;
|
||||
const Scalar _tmp32 = _tmp15 * _tmp30 - _tmp25 * (_tmp27 - _tmp28 - _tmp29);
|
||||
const Scalar _tmp33 = _tmp15 * (-_tmp18 - _tmp19 + _tmp20) - _tmp17 * _tmp25;
|
||||
const Scalar _tmp34 = _tmp25 * _tmp3;
|
||||
const Scalar _tmp35 = 2 * _tmp5;
|
||||
const Scalar _tmp36 = 2 * _tmp6;
|
||||
const Scalar _tmp37 = _tmp15 * (-_tmp35 + _tmp36) - _tmp34;
|
||||
const Scalar _tmp38 = _tmp15 * _tmp21;
|
||||
const Scalar _tmp39 = -_tmp25 * (_tmp35 + _tmp36) + _tmp38;
|
||||
const Scalar _tmp40 = _tmp15 * (2 * _tmp23 + 2 * _tmp24) - _tmp25 * (-2 * _tmp10 + 2 * _tmp11);
|
||||
const Scalar _tmp41 = -_tmp15 * _tmp22 + _tmp34;
|
||||
const Scalar _tmp42 = 2 * _tmp25 * _tmp7 - _tmp38;
|
||||
const Scalar _tmp43 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
|
||||
// Intermediate terms (40)
|
||||
const Scalar _tmp0 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp1 = 2 * state(3, 0);
|
||||
const Scalar _tmp2 = 2 * state(6, 0);
|
||||
const Scalar _tmp3 = _tmp2 * state(2, 0);
|
||||
const Scalar _tmp4 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp5 = _tmp4 - 2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp6 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp7 = _tmp1 * state(0, 0);
|
||||
const Scalar _tmp8 = 2 * state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp9 = _tmp7 + _tmp8;
|
||||
const Scalar _tmp10 = 2 * state(0, 0);
|
||||
const Scalar _tmp11 = _tmp1 * state(1, 0) - _tmp10 * state(2, 0);
|
||||
const Scalar _tmp12 = _tmp0 * _tmp9 + _tmp11 * state(6, 0) + _tmp5 * _tmp6;
|
||||
const Scalar _tmp13 =
|
||||
_tmp12 + epsilon * (2 * math::min<Scalar>(0, (((_tmp12) > 0) - ((_tmp12) < 0))) + 1);
|
||||
const Scalar _tmp14 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp15 = -_tmp7 + _tmp8;
|
||||
const Scalar _tmp16 = _tmp1 * state(2, 0) + _tmp10 * state(1, 0);
|
||||
const Scalar _tmp17 =
|
||||
(_tmp0 * _tmp14 + _tmp15 * _tmp6 + _tmp16 * state(6, 0)) / std::pow(_tmp13, Scalar(2));
|
||||
const Scalar _tmp18 = _tmp2 * state(1, 0);
|
||||
const Scalar _tmp19 = Scalar(1.0) / (_tmp13);
|
||||
const Scalar _tmp20 = -_tmp17 * (_tmp0 * _tmp1 - _tmp3) + _tmp19 * (-_tmp1 * _tmp6 + _tmp18);
|
||||
const Scalar _tmp21 = 2 * _tmp0;
|
||||
const Scalar _tmp22 = _tmp1 * state(6, 0);
|
||||
const Scalar _tmp23 = 4 * _tmp0;
|
||||
const Scalar _tmp24 = 2 * _tmp6;
|
||||
const Scalar _tmp25 = _tmp2 * state(0, 0);
|
||||
const Scalar _tmp26 = -_tmp17 * (_tmp21 * state(2, 0) + _tmp22) +
|
||||
_tmp19 * (-_tmp23 * state(1, 0) + _tmp24 * state(2, 0) + _tmp25);
|
||||
const Scalar _tmp27 = 4 * _tmp6;
|
||||
const Scalar _tmp28 = -_tmp17 * (_tmp21 * state(1, 0) - _tmp25 - _tmp27 * state(2, 0)) +
|
||||
_tmp19 * (_tmp22 + _tmp24 * state(1, 0));
|
||||
const Scalar _tmp29 = -_tmp17 * (_tmp18 + _tmp21 * state(0, 0) - _tmp27 * state(3, 0)) +
|
||||
_tmp19 * (-_tmp23 * state(3, 0) - _tmp24 * state(0, 0) + _tmp3);
|
||||
const Scalar _tmp30 = _tmp17 * _tmp5;
|
||||
const Scalar _tmp31 = _tmp15 * _tmp19;
|
||||
const Scalar _tmp32 = -_tmp30 + _tmp31;
|
||||
const Scalar _tmp33 = _tmp17 * _tmp9;
|
||||
const Scalar _tmp34 = _tmp14 * _tmp19;
|
||||
const Scalar _tmp35 = -_tmp33 + _tmp34;
|
||||
const Scalar _tmp36 = -_tmp11 * _tmp17 + _tmp16 * _tmp19;
|
||||
const Scalar _tmp37 = _tmp30 - _tmp31;
|
||||
const Scalar _tmp38 = _tmp33 - _tmp34;
|
||||
const Scalar _tmp39 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
|
||||
|
||||
// Output terms (2)
|
||||
if (H != nullptr) {
|
||||
@@ -88,92 +87,92 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp26;
|
||||
_h(1, 0) = _tmp31;
|
||||
_h(2, 0) = _tmp32;
|
||||
_h(3, 0) = _tmp33;
|
||||
_h(4, 0) = _tmp37;
|
||||
_h(5, 0) = _tmp39;
|
||||
_h(6, 0) = _tmp40;
|
||||
_h(22, 0) = _tmp41;
|
||||
_h(23, 0) = _tmp42;
|
||||
_h(0, 0) = _tmp20;
|
||||
_h(1, 0) = _tmp26;
|
||||
_h(2, 0) = _tmp28;
|
||||
_h(3, 0) = _tmp29;
|
||||
_h(4, 0) = _tmp32;
|
||||
_h(5, 0) = _tmp35;
|
||||
_h(6, 0) = _tmp36;
|
||||
_h(22, 0) = _tmp37;
|
||||
_h(23, 0) = _tmp38;
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
|
||||
|
||||
_k(0, 0) = _tmp43 * (P(0, 0) * _tmp26 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 +
|
||||
P(0, 22) * _tmp41 + P(0, 23) * _tmp42 + P(0, 3) * _tmp33 +
|
||||
P(0, 4) * _tmp37 + P(0, 5) * _tmp39 + P(0, 6) * _tmp40);
|
||||
_k(1, 0) = _tmp43 * (P(1, 0) * _tmp26 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 +
|
||||
P(1, 22) * _tmp41 + P(1, 23) * _tmp42 + P(1, 3) * _tmp33 +
|
||||
P(1, 4) * _tmp37 + P(1, 5) * _tmp39 + P(1, 6) * _tmp40);
|
||||
_k(2, 0) = _tmp43 * (P(2, 0) * _tmp26 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 +
|
||||
P(2, 22) * _tmp41 + P(2, 23) * _tmp42 + P(2, 3) * _tmp33 +
|
||||
P(2, 4) * _tmp37 + P(2, 5) * _tmp39 + P(2, 6) * _tmp40);
|
||||
_k(3, 0) = _tmp43 * (P(3, 0) * _tmp26 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 +
|
||||
P(3, 22) * _tmp41 + P(3, 23) * _tmp42 + P(3, 3) * _tmp33 +
|
||||
P(3, 4) * _tmp37 + P(3, 5) * _tmp39 + P(3, 6) * _tmp40);
|
||||
_k(4, 0) = _tmp43 * (P(4, 0) * _tmp26 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 +
|
||||
P(4, 22) * _tmp41 + P(4, 23) * _tmp42 + P(4, 3) * _tmp33 +
|
||||
P(4, 4) * _tmp37 + P(4, 5) * _tmp39 + P(4, 6) * _tmp40);
|
||||
_k(5, 0) = _tmp43 * (P(5, 0) * _tmp26 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 +
|
||||
P(5, 22) * _tmp41 + P(5, 23) * _tmp42 + P(5, 3) * _tmp33 +
|
||||
P(5, 4) * _tmp37 + P(5, 5) * _tmp39 + P(5, 6) * _tmp40);
|
||||
_k(6, 0) = _tmp43 * (P(6, 0) * _tmp26 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 +
|
||||
P(6, 22) * _tmp41 + P(6, 23) * _tmp42 + P(6, 3) * _tmp33 +
|
||||
P(6, 4) * _tmp37 + P(6, 5) * _tmp39 + P(6, 6) * _tmp40);
|
||||
_k(7, 0) = _tmp43 * (P(7, 0) * _tmp26 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 +
|
||||
P(7, 22) * _tmp41 + P(7, 23) * _tmp42 + P(7, 3) * _tmp33 +
|
||||
P(7, 4) * _tmp37 + P(7, 5) * _tmp39 + P(7, 6) * _tmp40);
|
||||
_k(8, 0) = _tmp43 * (P(8, 0) * _tmp26 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 +
|
||||
P(8, 22) * _tmp41 + P(8, 23) * _tmp42 + P(8, 3) * _tmp33 +
|
||||
P(8, 4) * _tmp37 + P(8, 5) * _tmp39 + P(8, 6) * _tmp40);
|
||||
_k(9, 0) = _tmp43 * (P(9, 0) * _tmp26 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 +
|
||||
P(9, 22) * _tmp41 + P(9, 23) * _tmp42 + P(9, 3) * _tmp33 +
|
||||
P(9, 4) * _tmp37 + P(9, 5) * _tmp39 + P(9, 6) * _tmp40);
|
||||
_k(10, 0) = _tmp43 * (P(10, 0) * _tmp26 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 +
|
||||
P(10, 22) * _tmp41 + P(10, 23) * _tmp42 + P(10, 3) * _tmp33 +
|
||||
P(10, 4) * _tmp37 + P(10, 5) * _tmp39 + P(10, 6) * _tmp40);
|
||||
_k(11, 0) = _tmp43 * (P(11, 0) * _tmp26 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 +
|
||||
P(11, 22) * _tmp41 + P(11, 23) * _tmp42 + P(11, 3) * _tmp33 +
|
||||
P(11, 4) * _tmp37 + P(11, 5) * _tmp39 + P(11, 6) * _tmp40);
|
||||
_k(12, 0) = _tmp43 * (P(12, 0) * _tmp26 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 +
|
||||
P(12, 22) * _tmp41 + P(12, 23) * _tmp42 + P(12, 3) * _tmp33 +
|
||||
P(12, 4) * _tmp37 + P(12, 5) * _tmp39 + P(12, 6) * _tmp40);
|
||||
_k(13, 0) = _tmp43 * (P(13, 0) * _tmp26 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 +
|
||||
P(13, 22) * _tmp41 + P(13, 23) * _tmp42 + P(13, 3) * _tmp33 +
|
||||
P(13, 4) * _tmp37 + P(13, 5) * _tmp39 + P(13, 6) * _tmp40);
|
||||
_k(14, 0) = _tmp43 * (P(14, 0) * _tmp26 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 +
|
||||
P(14, 22) * _tmp41 + P(14, 23) * _tmp42 + P(14, 3) * _tmp33 +
|
||||
P(14, 4) * _tmp37 + P(14, 5) * _tmp39 + P(14, 6) * _tmp40);
|
||||
_k(15, 0) = _tmp43 * (P(15, 0) * _tmp26 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 +
|
||||
P(15, 22) * _tmp41 + P(15, 23) * _tmp42 + P(15, 3) * _tmp33 +
|
||||
P(15, 4) * _tmp37 + P(15, 5) * _tmp39 + P(15, 6) * _tmp40);
|
||||
_k(16, 0) = _tmp43 * (P(16, 0) * _tmp26 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 +
|
||||
P(16, 22) * _tmp41 + P(16, 23) * _tmp42 + P(16, 3) * _tmp33 +
|
||||
P(16, 4) * _tmp37 + P(16, 5) * _tmp39 + P(16, 6) * _tmp40);
|
||||
_k(17, 0) = _tmp43 * (P(17, 0) * _tmp26 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 +
|
||||
P(17, 22) * _tmp41 + P(17, 23) * _tmp42 + P(17, 3) * _tmp33 +
|
||||
P(17, 4) * _tmp37 + P(17, 5) * _tmp39 + P(17, 6) * _tmp40);
|
||||
_k(18, 0) = _tmp43 * (P(18, 0) * _tmp26 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 +
|
||||
P(18, 22) * _tmp41 + P(18, 23) * _tmp42 + P(18, 3) * _tmp33 +
|
||||
P(18, 4) * _tmp37 + P(18, 5) * _tmp39 + P(18, 6) * _tmp40);
|
||||
_k(19, 0) = _tmp43 * (P(19, 0) * _tmp26 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 +
|
||||
P(19, 22) * _tmp41 + P(19, 23) * _tmp42 + P(19, 3) * _tmp33 +
|
||||
P(19, 4) * _tmp37 + P(19, 5) * _tmp39 + P(19, 6) * _tmp40);
|
||||
_k(20, 0) = _tmp43 * (P(20, 0) * _tmp26 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 +
|
||||
P(20, 22) * _tmp41 + P(20, 23) * _tmp42 + P(20, 3) * _tmp33 +
|
||||
P(20, 4) * _tmp37 + P(20, 5) * _tmp39 + P(20, 6) * _tmp40);
|
||||
_k(21, 0) = _tmp43 * (P(21, 0) * _tmp26 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 +
|
||||
P(21, 22) * _tmp41 + P(21, 23) * _tmp42 + P(21, 3) * _tmp33 +
|
||||
P(21, 4) * _tmp37 + P(21, 5) * _tmp39 + P(21, 6) * _tmp40);
|
||||
_k(22, 0) = _tmp43 * (P(22, 0) * _tmp26 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 +
|
||||
P(22, 22) * _tmp41 + P(22, 23) * _tmp42 + P(22, 3) * _tmp33 +
|
||||
P(22, 4) * _tmp37 + P(22, 5) * _tmp39 + P(22, 6) * _tmp40);
|
||||
_k(23, 0) = _tmp43 * (P(23, 0) * _tmp26 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 +
|
||||
P(23, 22) * _tmp41 + P(23, 23) * _tmp42 + P(23, 3) * _tmp33 +
|
||||
P(23, 4) * _tmp37 + P(23, 5) * _tmp39 + P(23, 6) * _tmp40);
|
||||
_k(0, 0) = _tmp39 * (P(0, 0) * _tmp20 + P(0, 1) * _tmp26 + P(0, 2) * _tmp28 +
|
||||
P(0, 22) * _tmp37 + P(0, 23) * _tmp38 + P(0, 3) * _tmp29 +
|
||||
P(0, 4) * _tmp32 + P(0, 5) * _tmp35 + P(0, 6) * _tmp36);
|
||||
_k(1, 0) = _tmp39 * (P(1, 0) * _tmp20 + P(1, 1) * _tmp26 + P(1, 2) * _tmp28 +
|
||||
P(1, 22) * _tmp37 + P(1, 23) * _tmp38 + P(1, 3) * _tmp29 +
|
||||
P(1, 4) * _tmp32 + P(1, 5) * _tmp35 + P(1, 6) * _tmp36);
|
||||
_k(2, 0) = _tmp39 * (P(2, 0) * _tmp20 + P(2, 1) * _tmp26 + P(2, 2) * _tmp28 +
|
||||
P(2, 22) * _tmp37 + P(2, 23) * _tmp38 + P(2, 3) * _tmp29 +
|
||||
P(2, 4) * _tmp32 + P(2, 5) * _tmp35 + P(2, 6) * _tmp36);
|
||||
_k(3, 0) = _tmp39 * (P(3, 0) * _tmp20 + P(3, 1) * _tmp26 + P(3, 2) * _tmp28 +
|
||||
P(3, 22) * _tmp37 + P(3, 23) * _tmp38 + P(3, 3) * _tmp29 +
|
||||
P(3, 4) * _tmp32 + P(3, 5) * _tmp35 + P(3, 6) * _tmp36);
|
||||
_k(4, 0) = _tmp39 * (P(4, 0) * _tmp20 + P(4, 1) * _tmp26 + P(4, 2) * _tmp28 +
|
||||
P(4, 22) * _tmp37 + P(4, 23) * _tmp38 + P(4, 3) * _tmp29 +
|
||||
P(4, 4) * _tmp32 + P(4, 5) * _tmp35 + P(4, 6) * _tmp36);
|
||||
_k(5, 0) = _tmp39 * (P(5, 0) * _tmp20 + P(5, 1) * _tmp26 + P(5, 2) * _tmp28 +
|
||||
P(5, 22) * _tmp37 + P(5, 23) * _tmp38 + P(5, 3) * _tmp29 +
|
||||
P(5, 4) * _tmp32 + P(5, 5) * _tmp35 + P(5, 6) * _tmp36);
|
||||
_k(6, 0) = _tmp39 * (P(6, 0) * _tmp20 + P(6, 1) * _tmp26 + P(6, 2) * _tmp28 +
|
||||
P(6, 22) * _tmp37 + P(6, 23) * _tmp38 + P(6, 3) * _tmp29 +
|
||||
P(6, 4) * _tmp32 + P(6, 5) * _tmp35 + P(6, 6) * _tmp36);
|
||||
_k(7, 0) = _tmp39 * (P(7, 0) * _tmp20 + P(7, 1) * _tmp26 + P(7, 2) * _tmp28 +
|
||||
P(7, 22) * _tmp37 + P(7, 23) * _tmp38 + P(7, 3) * _tmp29 +
|
||||
P(7, 4) * _tmp32 + P(7, 5) * _tmp35 + P(7, 6) * _tmp36);
|
||||
_k(8, 0) = _tmp39 * (P(8, 0) * _tmp20 + P(8, 1) * _tmp26 + P(8, 2) * _tmp28 +
|
||||
P(8, 22) * _tmp37 + P(8, 23) * _tmp38 + P(8, 3) * _tmp29 +
|
||||
P(8, 4) * _tmp32 + P(8, 5) * _tmp35 + P(8, 6) * _tmp36);
|
||||
_k(9, 0) = _tmp39 * (P(9, 0) * _tmp20 + P(9, 1) * _tmp26 + P(9, 2) * _tmp28 +
|
||||
P(9, 22) * _tmp37 + P(9, 23) * _tmp38 + P(9, 3) * _tmp29 +
|
||||
P(9, 4) * _tmp32 + P(9, 5) * _tmp35 + P(9, 6) * _tmp36);
|
||||
_k(10, 0) = _tmp39 * (P(10, 0) * _tmp20 + P(10, 1) * _tmp26 + P(10, 2) * _tmp28 +
|
||||
P(10, 22) * _tmp37 + P(10, 23) * _tmp38 + P(10, 3) * _tmp29 +
|
||||
P(10, 4) * _tmp32 + P(10, 5) * _tmp35 + P(10, 6) * _tmp36);
|
||||
_k(11, 0) = _tmp39 * (P(11, 0) * _tmp20 + P(11, 1) * _tmp26 + P(11, 2) * _tmp28 +
|
||||
P(11, 22) * _tmp37 + P(11, 23) * _tmp38 + P(11, 3) * _tmp29 +
|
||||
P(11, 4) * _tmp32 + P(11, 5) * _tmp35 + P(11, 6) * _tmp36);
|
||||
_k(12, 0) = _tmp39 * (P(12, 0) * _tmp20 + P(12, 1) * _tmp26 + P(12, 2) * _tmp28 +
|
||||
P(12, 22) * _tmp37 + P(12, 23) * _tmp38 + P(12, 3) * _tmp29 +
|
||||
P(12, 4) * _tmp32 + P(12, 5) * _tmp35 + P(12, 6) * _tmp36);
|
||||
_k(13, 0) = _tmp39 * (P(13, 0) * _tmp20 + P(13, 1) * _tmp26 + P(13, 2) * _tmp28 +
|
||||
P(13, 22) * _tmp37 + P(13, 23) * _tmp38 + P(13, 3) * _tmp29 +
|
||||
P(13, 4) * _tmp32 + P(13, 5) * _tmp35 + P(13, 6) * _tmp36);
|
||||
_k(14, 0) = _tmp39 * (P(14, 0) * _tmp20 + P(14, 1) * _tmp26 + P(14, 2) * _tmp28 +
|
||||
P(14, 22) * _tmp37 + P(14, 23) * _tmp38 + P(14, 3) * _tmp29 +
|
||||
P(14, 4) * _tmp32 + P(14, 5) * _tmp35 + P(14, 6) * _tmp36);
|
||||
_k(15, 0) = _tmp39 * (P(15, 0) * _tmp20 + P(15, 1) * _tmp26 + P(15, 2) * _tmp28 +
|
||||
P(15, 22) * _tmp37 + P(15, 23) * _tmp38 + P(15, 3) * _tmp29 +
|
||||
P(15, 4) * _tmp32 + P(15, 5) * _tmp35 + P(15, 6) * _tmp36);
|
||||
_k(16, 0) = _tmp39 * (P(16, 0) * _tmp20 + P(16, 1) * _tmp26 + P(16, 2) * _tmp28 +
|
||||
P(16, 22) * _tmp37 + P(16, 23) * _tmp38 + P(16, 3) * _tmp29 +
|
||||
P(16, 4) * _tmp32 + P(16, 5) * _tmp35 + P(16, 6) * _tmp36);
|
||||
_k(17, 0) = _tmp39 * (P(17, 0) * _tmp20 + P(17, 1) * _tmp26 + P(17, 2) * _tmp28 +
|
||||
P(17, 22) * _tmp37 + P(17, 23) * _tmp38 + P(17, 3) * _tmp29 +
|
||||
P(17, 4) * _tmp32 + P(17, 5) * _tmp35 + P(17, 6) * _tmp36);
|
||||
_k(18, 0) = _tmp39 * (P(18, 0) * _tmp20 + P(18, 1) * _tmp26 + P(18, 2) * _tmp28 +
|
||||
P(18, 22) * _tmp37 + P(18, 23) * _tmp38 + P(18, 3) * _tmp29 +
|
||||
P(18, 4) * _tmp32 + P(18, 5) * _tmp35 + P(18, 6) * _tmp36);
|
||||
_k(19, 0) = _tmp39 * (P(19, 0) * _tmp20 + P(19, 1) * _tmp26 + P(19, 2) * _tmp28 +
|
||||
P(19, 22) * _tmp37 + P(19, 23) * _tmp38 + P(19, 3) * _tmp29 +
|
||||
P(19, 4) * _tmp32 + P(19, 5) * _tmp35 + P(19, 6) * _tmp36);
|
||||
_k(20, 0) = _tmp39 * (P(20, 0) * _tmp20 + P(20, 1) * _tmp26 + P(20, 2) * _tmp28 +
|
||||
P(20, 22) * _tmp37 + P(20, 23) * _tmp38 + P(20, 3) * _tmp29 +
|
||||
P(20, 4) * _tmp32 + P(20, 5) * _tmp35 + P(20, 6) * _tmp36);
|
||||
_k(21, 0) = _tmp39 * (P(21, 0) * _tmp20 + P(21, 1) * _tmp26 + P(21, 2) * _tmp28 +
|
||||
P(21, 22) * _tmp37 + P(21, 23) * _tmp38 + P(21, 3) * _tmp29 +
|
||||
P(21, 4) * _tmp32 + P(21, 5) * _tmp35 + P(21, 6) * _tmp36);
|
||||
_k(22, 0) = _tmp39 * (P(22, 0) * _tmp20 + P(22, 1) * _tmp26 + P(22, 2) * _tmp28 +
|
||||
P(22, 22) * _tmp37 + P(22, 23) * _tmp38 + P(22, 3) * _tmp29 +
|
||||
P(22, 4) * _tmp32 + P(22, 5) * _tmp35 + P(22, 6) * _tmp36);
|
||||
_k(23, 0) = _tmp39 * (P(23, 0) * _tmp20 + P(23, 1) * _tmp26 + P(23, 2) * _tmp28 +
|
||||
P(23, 22) * _tmp37 + P(23, 23) * _tmp38 + P(23, 3) * _tmp29 +
|
||||
P(23, 4) * _tmp32 + P(23, 5) * _tmp35 + P(23, 6) * _tmp36);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+72
-74
@@ -30,95 +30,93 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov = nullptr,
|
||||
Scalar* const innov_var = nullptr) {
|
||||
// Total ops: 276
|
||||
// Total ops: 269
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (44)
|
||||
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 = -_tmp0 + _tmp1 + _tmp2;
|
||||
const Scalar _tmp4 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp5 = state(0, 0) * state(3, 0);
|
||||
const Scalar _tmp6 = state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp7 = _tmp5 + _tmp6;
|
||||
// Intermediate terms (39)
|
||||
const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = _tmp0 - 2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp2 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp3 = 2 * state(3, 0);
|
||||
const Scalar _tmp4 = _tmp3 * state(0, 0);
|
||||
const Scalar _tmp5 = 2 * state(1, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(2, 0);
|
||||
const Scalar _tmp7 = _tmp4 + _tmp6;
|
||||
const Scalar _tmp8 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp9 = 2 * _tmp8;
|
||||
const Scalar _tmp10 = state(0, 0) * state(2, 0);
|
||||
const Scalar _tmp11 = state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp12 = 2 * state(6, 0);
|
||||
const Scalar _tmp13 = _tmp12 * (-_tmp10 + _tmp11) + _tmp3 * _tmp4 + _tmp7 * _tmp9;
|
||||
const Scalar _tmp14 =
|
||||
_tmp13 + epsilon * (2 * math::min<Scalar>(0, (((_tmp13) > 0) - ((_tmp13) < 0))) + 1);
|
||||
const Scalar _tmp15 = Scalar(1.0) / (_tmp14);
|
||||
const Scalar _tmp16 = _tmp0 - _tmp1 + _tmp2;
|
||||
const Scalar _tmp17 = -_tmp5 + _tmp6;
|
||||
const Scalar _tmp18 = 2 * _tmp4;
|
||||
const Scalar _tmp19 = state(2, 0) * state(3, 0);
|
||||
const Scalar _tmp20 = state(0, 0) * state(1, 0);
|
||||
const Scalar _tmp21 = _tmp12 * (_tmp19 + _tmp20) + _tmp16 * _tmp8 + _tmp17 * _tmp18;
|
||||
const Scalar _tmp22 = _tmp9 * state(1, 0);
|
||||
const Scalar _tmp23 = _tmp18 * state(2, 0);
|
||||
const Scalar _tmp24 = _tmp12 * state(0, 0);
|
||||
const Scalar _tmp25 = _tmp12 * state(3, 0) + _tmp18 * state(1, 0) + _tmp9 * state(2, 0);
|
||||
const Scalar _tmp26 = _tmp21 / std::pow(_tmp14, Scalar(2));
|
||||
const Scalar _tmp27 = _tmp15 * (-_tmp22 + _tmp23 + _tmp24) - _tmp25 * _tmp26;
|
||||
const Scalar _tmp28 = _tmp15 * _tmp16;
|
||||
const Scalar _tmp29 = 2 * _tmp5;
|
||||
const Scalar _tmp30 = 2 * _tmp6;
|
||||
const Scalar _tmp31 = -_tmp26 * (_tmp29 + _tmp30) + _tmp28;
|
||||
const Scalar _tmp32 = _tmp12 * state(1, 0) - _tmp18 * state(3, 0) + _tmp9 * state(0, 0);
|
||||
const Scalar _tmp33 = _tmp18 * state(0, 0);
|
||||
const Scalar _tmp34 = _tmp9 * state(3, 0);
|
||||
const Scalar _tmp35 = _tmp12 * state(2, 0);
|
||||
const Scalar _tmp36 = _tmp15 * _tmp32 - _tmp26 * (_tmp33 + _tmp34 - _tmp35);
|
||||
const Scalar _tmp37 = 2 * _tmp26 * _tmp7 - _tmp28;
|
||||
const Scalar _tmp38 = _tmp26 * _tmp3;
|
||||
const Scalar _tmp39 = -2 * _tmp15 * _tmp17 + _tmp38;
|
||||
const Scalar _tmp40 = _tmp15 * _tmp25 - _tmp26 * (_tmp22 - _tmp23 - _tmp24);
|
||||
const Scalar _tmp41 = _tmp15 * (-_tmp33 - _tmp34 + _tmp35) - _tmp26 * _tmp32;
|
||||
const Scalar _tmp42 = _tmp15 * (2 * _tmp19 + 2 * _tmp20) - _tmp26 * (-2 * _tmp10 + 2 * _tmp11);
|
||||
const Scalar _tmp43 = _tmp15 * (-_tmp29 + _tmp30) - _tmp38;
|
||||
const Scalar _tmp9 = 2 * state(2, 0);
|
||||
const Scalar _tmp10 = _tmp3 * state(1, 0) - _tmp9 * state(0, 0);
|
||||
const Scalar _tmp11 = _tmp1 * _tmp2 + _tmp10 * state(6, 0) + _tmp7 * _tmp8;
|
||||
const Scalar _tmp12 =
|
||||
_tmp11 + epsilon * (2 * math::min<Scalar>(0, (((_tmp11) > 0) - ((_tmp11) < 0))) + 1);
|
||||
const Scalar _tmp13 = Scalar(1.0) / (_tmp12);
|
||||
const Scalar _tmp14 = _tmp0 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp15 = -_tmp4 + _tmp6;
|
||||
const Scalar _tmp16 = _tmp3 * state(2, 0) + _tmp5 * state(0, 0);
|
||||
const Scalar _tmp17 = _tmp14 * _tmp8 + _tmp15 * _tmp2 + _tmp16 * state(6, 0);
|
||||
const Scalar _tmp18 = _tmp17 / std::pow(_tmp12, Scalar(2));
|
||||
const Scalar _tmp19 = _tmp1 * _tmp18;
|
||||
const Scalar _tmp20 = _tmp13 * _tmp15;
|
||||
const Scalar _tmp21 = -_tmp19 + _tmp20;
|
||||
const Scalar _tmp22 = _tmp3 * state(6, 0);
|
||||
const Scalar _tmp23 = 4 * _tmp8;
|
||||
const Scalar _tmp24 = 2 * state(0, 0);
|
||||
const Scalar _tmp25 = _tmp24 * state(6, 0);
|
||||
const Scalar _tmp26 =
|
||||
_tmp13 * (_tmp2 * _tmp9 - _tmp23 * state(1, 0) + _tmp25) - _tmp18 * (_tmp22 + _tmp8 * _tmp9);
|
||||
const Scalar _tmp27 = -_tmp10 * _tmp18 + _tmp13 * _tmp16;
|
||||
const Scalar _tmp28 = 4 * _tmp2;
|
||||
const Scalar _tmp29 =
|
||||
_tmp13 * (_tmp2 * _tmp5 + _tmp22) - _tmp18 * (-_tmp25 - _tmp28 * state(2, 0) + _tmp5 * _tmp8);
|
||||
const Scalar _tmp30 = _tmp9 * state(6, 0);
|
||||
const Scalar _tmp31 = _tmp5 * state(6, 0);
|
||||
const Scalar _tmp32 = _tmp13 * (-_tmp2 * _tmp3 + _tmp31) - _tmp18 * (_tmp3 * _tmp8 - _tmp30);
|
||||
const Scalar _tmp33 = _tmp19 - _tmp20;
|
||||
const Scalar _tmp34 = _tmp18 * _tmp7;
|
||||
const Scalar _tmp35 = _tmp13 * _tmp14;
|
||||
const Scalar _tmp36 = -_tmp34 + _tmp35;
|
||||
const Scalar _tmp37 = _tmp34 - _tmp35;
|
||||
const Scalar _tmp38 = _tmp13 * (-_tmp2 * _tmp24 - _tmp23 * state(3, 0) + _tmp30) -
|
||||
_tmp18 * (_tmp24 * _tmp8 - _tmp28 * state(3, 0) + _tmp31);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov != nullptr) {
|
||||
Scalar& _innov = (*innov);
|
||||
|
||||
_innov = _tmp15 * _tmp21;
|
||||
_innov = _tmp13 * _tmp17;
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R +
|
||||
_tmp27 * (P(0, 1) * _tmp36 + P(1, 1) * _tmp27 + P(2, 1) * _tmp40 +
|
||||
P(22, 1) * _tmp39 + P(23, 1) * _tmp37 + P(3, 1) * _tmp41 +
|
||||
P(4, 1) * _tmp43 + P(5, 1) * _tmp31 + P(6, 1) * _tmp42) +
|
||||
_tmp31 * (P(0, 5) * _tmp36 + P(1, 5) * _tmp27 + P(2, 5) * _tmp40 +
|
||||
P(22, 5) * _tmp39 + P(23, 5) * _tmp37 + P(3, 5) * _tmp41 +
|
||||
P(4, 5) * _tmp43 + P(5, 5) * _tmp31 + P(6, 5) * _tmp42) +
|
||||
_tmp36 * (P(0, 0) * _tmp36 + P(1, 0) * _tmp27 + P(2, 0) * _tmp40 +
|
||||
P(22, 0) * _tmp39 + P(23, 0) * _tmp37 + P(3, 0) * _tmp41 +
|
||||
P(4, 0) * _tmp43 + P(5, 0) * _tmp31 + P(6, 0) * _tmp42) +
|
||||
_tmp37 * (P(0, 23) * _tmp36 + P(1, 23) * _tmp27 + P(2, 23) * _tmp40 +
|
||||
P(22, 23) * _tmp39 + P(23, 23) * _tmp37 + P(3, 23) * _tmp41 +
|
||||
P(4, 23) * _tmp43 + P(5, 23) * _tmp31 + P(6, 23) * _tmp42) +
|
||||
_tmp39 * (P(0, 22) * _tmp36 + P(1, 22) * _tmp27 + P(2, 22) * _tmp40 +
|
||||
P(22, 22) * _tmp39 + P(23, 22) * _tmp37 + P(3, 22) * _tmp41 +
|
||||
P(4, 22) * _tmp43 + P(5, 22) * _tmp31 + P(6, 22) * _tmp42) +
|
||||
_tmp40 * (P(0, 2) * _tmp36 + P(1, 2) * _tmp27 + P(2, 2) * _tmp40 +
|
||||
P(22, 2) * _tmp39 + P(23, 2) * _tmp37 + P(3, 2) * _tmp41 +
|
||||
P(4, 2) * _tmp43 + P(5, 2) * _tmp31 + P(6, 2) * _tmp42) +
|
||||
_tmp41 * (P(0, 3) * _tmp36 + P(1, 3) * _tmp27 + P(2, 3) * _tmp40 +
|
||||
P(22, 3) * _tmp39 + P(23, 3) * _tmp37 + P(3, 3) * _tmp41 +
|
||||
P(4, 3) * _tmp43 + P(5, 3) * _tmp31 + P(6, 3) * _tmp42) +
|
||||
_tmp42 * (P(0, 6) * _tmp36 + P(1, 6) * _tmp27 + P(2, 6) * _tmp40 +
|
||||
P(22, 6) * _tmp39 + P(23, 6) * _tmp37 + P(3, 6) * _tmp41 +
|
||||
P(4, 6) * _tmp43 + P(5, 6) * _tmp31 + P(6, 6) * _tmp42) +
|
||||
_tmp43 * (P(0, 4) * _tmp36 + P(1, 4) * _tmp27 + P(2, 4) * _tmp40 +
|
||||
P(22, 4) * _tmp39 + P(23, 4) * _tmp37 + P(3, 4) * _tmp41 +
|
||||
P(4, 4) * _tmp43 + P(5, 4) * _tmp31 + P(6, 4) * _tmp42);
|
||||
_tmp21 * (P(0, 4) * _tmp32 + P(1, 4) * _tmp26 + P(2, 4) * _tmp29 +
|
||||
P(22, 4) * _tmp33 + P(23, 4) * _tmp37 + P(3, 4) * _tmp38 +
|
||||
P(4, 4) * _tmp21 + P(5, 4) * _tmp36 + P(6, 4) * _tmp27) +
|
||||
_tmp26 * (P(0, 1) * _tmp32 + P(1, 1) * _tmp26 + P(2, 1) * _tmp29 +
|
||||
P(22, 1) * _tmp33 + P(23, 1) * _tmp37 + P(3, 1) * _tmp38 +
|
||||
P(4, 1) * _tmp21 + P(5, 1) * _tmp36 + P(6, 1) * _tmp27) +
|
||||
_tmp27 * (P(0, 6) * _tmp32 + P(1, 6) * _tmp26 + P(2, 6) * _tmp29 +
|
||||
P(22, 6) * _tmp33 + P(23, 6) * _tmp37 + P(3, 6) * _tmp38 +
|
||||
P(4, 6) * _tmp21 + P(5, 6) * _tmp36 + P(6, 6) * _tmp27) +
|
||||
_tmp29 * (P(0, 2) * _tmp32 + P(1, 2) * _tmp26 + P(2, 2) * _tmp29 +
|
||||
P(22, 2) * _tmp33 + P(23, 2) * _tmp37 + P(3, 2) * _tmp38 +
|
||||
P(4, 2) * _tmp21 + P(5, 2) * _tmp36 + P(6, 2) * _tmp27) +
|
||||
_tmp32 * (P(0, 0) * _tmp32 + P(1, 0) * _tmp26 + P(2, 0) * _tmp29 +
|
||||
P(22, 0) * _tmp33 + P(23, 0) * _tmp37 + P(3, 0) * _tmp38 +
|
||||
P(4, 0) * _tmp21 + P(5, 0) * _tmp36 + P(6, 0) * _tmp27) +
|
||||
_tmp33 * (P(0, 22) * _tmp32 + P(1, 22) * _tmp26 + P(2, 22) * _tmp29 +
|
||||
P(22, 22) * _tmp33 + P(23, 22) * _tmp37 + P(3, 22) * _tmp38 +
|
||||
P(4, 22) * _tmp21 + P(5, 22) * _tmp36 + P(6, 22) * _tmp27) +
|
||||
_tmp36 * (P(0, 5) * _tmp32 + P(1, 5) * _tmp26 + P(2, 5) * _tmp29 +
|
||||
P(22, 5) * _tmp33 + P(23, 5) * _tmp37 + P(3, 5) * _tmp38 +
|
||||
P(4, 5) * _tmp21 + P(5, 5) * _tmp36 + P(6, 5) * _tmp27) +
|
||||
_tmp37 * (P(0, 23) * _tmp32 + P(1, 23) * _tmp26 + P(2, 23) * _tmp29 +
|
||||
P(22, 23) * _tmp33 + P(23, 23) * _tmp37 + P(3, 23) * _tmp38 +
|
||||
P(4, 23) * _tmp21 + P(5, 23) * _tmp36 + P(6, 23) * _tmp27) +
|
||||
_tmp38 * (P(0, 3) * _tmp32 + P(1, 3) * _tmp26 + P(2, 3) * _tmp29 +
|
||||
P(22, 3) * _tmp33 + P(23, 3) * _tmp37 + P(3, 3) * _tmp38 +
|
||||
P(4, 3) * _tmp21 + P(5, 3) * _tmp36 + P(6, 3) * _tmp27);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+27
-23
@@ -30,33 +30,37 @@ void ComputeYaw312InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 75
|
||||
// Total ops: 73
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (11)
|
||||
const Scalar _tmp0 = -state(0, 0) * state(3, 0) + state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp1 = 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 = _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp3 = std::pow(_tmp2, Scalar(2));
|
||||
const Scalar _tmp4 = 4 * _tmp0 / _tmp3;
|
||||
const Scalar _tmp5 = 2 / _tmp2;
|
||||
const Scalar _tmp6 = _tmp3 / (4 * std::pow(_tmp0, Scalar(2)) + _tmp3);
|
||||
const Scalar _tmp7 = _tmp6 * (-_tmp4 * state(1, 0) - _tmp5 * state(2, 0));
|
||||
const Scalar _tmp8 = _tmp6 * (_tmp4 * state(2, 0) - _tmp5 * state(1, 0));
|
||||
const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(3, 0) + _tmp5 * state(0, 0));
|
||||
const Scalar _tmp10 = _tmp6 * (_tmp4 * state(0, 0) + _tmp5 * state(3, 0));
|
||||
// Intermediate terms (15)
|
||||
const Scalar _tmp0 = 2 * state(0, 0);
|
||||
const Scalar _tmp1 = 2 * state(2, 0);
|
||||
const Scalar _tmp2 = -_tmp0 * state(3, 0) + _tmp1 * state(1, 0);
|
||||
const Scalar _tmp3 =
|
||||
-2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
|
||||
const Scalar _tmp4 = _tmp3 + epsilon * ((((_tmp3) > 0) - ((_tmp3) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp5 = std::pow(_tmp4, Scalar(2));
|
||||
const Scalar _tmp6 = 4 * _tmp2 / _tmp5;
|
||||
const Scalar _tmp7 = Scalar(1.0) / (_tmp4);
|
||||
const Scalar _tmp8 = Scalar(1.0) / (std::pow(_tmp2, Scalar(2)) + _tmp5);
|
||||
const Scalar _tmp9 = _tmp5 * _tmp8;
|
||||
const Scalar _tmp10 = _tmp9 * (-_tmp1 * _tmp7 - _tmp6 * state(1, 0));
|
||||
const Scalar _tmp11 = _tmp9 * (_tmp0 * _tmp7 - _tmp6 * state(3, 0));
|
||||
const Scalar _tmp12 = 2 * _tmp4 * _tmp8;
|
||||
const Scalar _tmp13 = _tmp12 * state(3, 0);
|
||||
const Scalar _tmp14 = _tmp12 * state(1, 0);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R +
|
||||
_tmp10 * (P(0, 0) * _tmp10 + P(1, 0) * _tmp7 + P(2, 0) * _tmp8 + P(3, 0) * _tmp9) +
|
||||
_tmp7 * (P(0, 1) * _tmp10 + P(1, 1) * _tmp7 + P(2, 1) * _tmp8 + P(3, 1) * _tmp9) +
|
||||
_tmp8 * (P(0, 2) * _tmp10 + P(1, 2) * _tmp7 + P(2, 2) * _tmp8 + P(3, 2) * _tmp9) +
|
||||
_tmp9 * (P(0, 3) * _tmp10 + P(1, 3) * _tmp7 + P(2, 3) * _tmp8 + P(3, 3) * _tmp9);
|
||||
_innov_var =
|
||||
R + _tmp10 * (P(0, 1) * _tmp13 + P(1, 1) * _tmp10 - P(2, 1) * _tmp14 + P(3, 1) * _tmp11) +
|
||||
_tmp11 * (P(0, 3) * _tmp13 + P(1, 3) * _tmp10 - P(2, 3) * _tmp14 + P(3, 3) * _tmp11) +
|
||||
_tmp13 * (P(0, 0) * _tmp13 + P(1, 0) * _tmp10 - P(2, 0) * _tmp14 + P(3, 0) * _tmp11) -
|
||||
_tmp14 * (P(0, 2) * _tmp13 + P(1, 2) * _tmp10 - P(2, 2) * _tmp14 + P(3, 2) * _tmp11);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
@@ -64,10 +68,10 @@ void ComputeYaw312InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp10;
|
||||
_h(1, 0) = _tmp7;
|
||||
_h(2, 0) = _tmp8;
|
||||
_h(3, 0) = _tmp9;
|
||||
_h(0, 0) = _tmp13;
|
||||
_h(1, 0) = _tmp10;
|
||||
_h(2, 0) = -_tmp14;
|
||||
_h(3, 0) = _tmp11;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+26
-22
@@ -30,33 +30,37 @@ void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& sta
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 83
|
||||
// Total ops: 73
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (11)
|
||||
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 = -state(0, 0) * state(3, 0) + state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp2 = 2 * _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp3 = std::pow(_tmp2, Scalar(2));
|
||||
const Scalar _tmp4 = 2 * _tmp0 / _tmp3;
|
||||
const Scalar _tmp5 = 2 / _tmp2;
|
||||
const Scalar _tmp6 = _tmp3 / (std::pow(_tmp0, Scalar(2)) + _tmp3);
|
||||
const Scalar _tmp7 = _tmp6 * (_tmp4 * state(1, 0) - _tmp5 * state(2, 0));
|
||||
const Scalar _tmp8 = _tmp6 * (_tmp4 * state(2, 0) + _tmp5 * state(1, 0));
|
||||
const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(0, 0) + _tmp5 * state(3, 0));
|
||||
const Scalar _tmp10 = _tmp6 * (-_tmp4 * state(3, 0) - _tmp5 * state(0, 0));
|
||||
// Intermediate terms (15)
|
||||
const Scalar _tmp0 = 2 * state(0, 0);
|
||||
const Scalar _tmp1 =
|
||||
-2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
|
||||
const Scalar _tmp2 = 2 * state(2, 0);
|
||||
const Scalar _tmp3 = -_tmp0 * state(3, 0) + _tmp2 * state(1, 0);
|
||||
const Scalar _tmp4 = _tmp3 + epsilon * ((((_tmp3) > 0) - ((_tmp3) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp5 = std::pow(_tmp4, Scalar(2));
|
||||
const Scalar _tmp6 = _tmp1 / _tmp5;
|
||||
const Scalar _tmp7 = 4 / _tmp4;
|
||||
const Scalar _tmp8 = Scalar(1.0) / (std::pow(_tmp1, Scalar(2)) + _tmp5);
|
||||
const Scalar _tmp9 = _tmp5 * _tmp8;
|
||||
const Scalar _tmp10 = _tmp9 * (-_tmp0 * _tmp6 + _tmp7 * state(3, 0));
|
||||
const Scalar _tmp11 = 2 * _tmp1 * _tmp8;
|
||||
const Scalar _tmp12 = _tmp11 * state(3, 0);
|
||||
const Scalar _tmp13 = _tmp11 * state(1, 0);
|
||||
const Scalar _tmp14 = _tmp9 * (_tmp2 * _tmp6 + _tmp7 * state(1, 0));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var =
|
||||
R - _tmp10 * (-P(0, 0) * _tmp10 - P(1, 0) * _tmp8 - P(2, 0) * _tmp7 - P(3, 0) * _tmp9) -
|
||||
_tmp7 * (-P(0, 2) * _tmp10 - P(1, 2) * _tmp8 - P(2, 2) * _tmp7 - P(3, 2) * _tmp9) -
|
||||
_tmp8 * (-P(0, 1) * _tmp10 - P(1, 1) * _tmp8 - P(2, 1) * _tmp7 - P(3, 1) * _tmp9) -
|
||||
_tmp9 * (-P(0, 3) * _tmp10 - P(1, 3) * _tmp8 - P(2, 3) * _tmp7 - P(3, 3) * _tmp9);
|
||||
R - _tmp10 * (P(0, 3) * _tmp12 - P(1, 3) * _tmp14 - P(2, 3) * _tmp13 - P(3, 3) * _tmp10) +
|
||||
_tmp12 * (P(0, 0) * _tmp12 - P(1, 0) * _tmp14 - P(2, 0) * _tmp13 - P(3, 0) * _tmp10) -
|
||||
_tmp13 * (P(0, 2) * _tmp12 - P(1, 2) * _tmp14 - P(2, 2) * _tmp13 - P(3, 2) * _tmp10) -
|
||||
_tmp14 * (P(0, 1) * _tmp12 - P(1, 1) * _tmp14 - P(2, 1) * _tmp13 - P(3, 1) * _tmp10);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
@@ -64,10 +68,10 @@ void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& sta
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = -_tmp10;
|
||||
_h(1, 0) = -_tmp8;
|
||||
_h(2, 0) = -_tmp7;
|
||||
_h(3, 0) = -_tmp9;
|
||||
_h(0, 0) = _tmp12;
|
||||
_h(1, 0) = -_tmp14;
|
||||
_h(2, 0) = -_tmp13;
|
||||
_h(3, 0) = -_tmp10;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+27
-23
@@ -30,33 +30,37 @@ void ComputeYaw321InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 74
|
||||
// Total ops: 70
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (11)
|
||||
const Scalar _tmp0 = state(0, 0) * state(3, 0) + state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp1 = 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 = _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp3 = std::pow(_tmp2, Scalar(2));
|
||||
const Scalar _tmp4 = 4 * _tmp0 / _tmp3;
|
||||
const Scalar _tmp5 = 2 / _tmp2;
|
||||
const Scalar _tmp6 = _tmp3 / (4 * std::pow(_tmp0, Scalar(2)) + _tmp3);
|
||||
const Scalar _tmp7 = _tmp6 * (_tmp4 * state(2, 0) + _tmp5 * state(1, 0));
|
||||
const Scalar _tmp8 = _tmp6 * (-_tmp4 * state(1, 0) + _tmp5 * state(2, 0));
|
||||
const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(0, 0) + _tmp5 * state(3, 0));
|
||||
const Scalar _tmp10 = _tmp6 * (_tmp4 * state(3, 0) + _tmp5 * state(0, 0));
|
||||
// Intermediate terms (15)
|
||||
const Scalar _tmp0 = 2 * state(3, 0);
|
||||
const Scalar _tmp1 = 2 * state(2, 0);
|
||||
const Scalar _tmp2 = _tmp0 * state(0, 0) + _tmp1 * state(1, 0);
|
||||
const Scalar _tmp3 =
|
||||
-2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
|
||||
const Scalar _tmp4 = _tmp3 + epsilon * ((((_tmp3) > 0) - ((_tmp3) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp5 = std::pow(_tmp4, Scalar(2));
|
||||
const Scalar _tmp6 = Scalar(1.0) / (std::pow(_tmp2, Scalar(2)) + _tmp5);
|
||||
const Scalar _tmp7 = _tmp4 * _tmp6;
|
||||
const Scalar _tmp8 = _tmp0 * _tmp7;
|
||||
const Scalar _tmp9 = _tmp1 * _tmp7;
|
||||
const Scalar _tmp10 = 4 * _tmp2 / _tmp5;
|
||||
const Scalar _tmp11 = 2 / _tmp4;
|
||||
const Scalar _tmp12 = _tmp5 * _tmp6;
|
||||
const Scalar _tmp13 = _tmp12 * (_tmp10 * state(3, 0) + _tmp11 * state(0, 0));
|
||||
const Scalar _tmp14 = _tmp12 * (_tmp10 * state(2, 0) + _tmp11 * state(1, 0));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R +
|
||||
_tmp10 * (P(0, 3) * _tmp9 + P(1, 3) * _tmp8 + P(2, 3) * _tmp7 + P(3, 3) * _tmp10) +
|
||||
_tmp7 * (P(0, 2) * _tmp9 + P(1, 2) * _tmp8 + P(2, 2) * _tmp7 + P(3, 2) * _tmp10) +
|
||||
_tmp8 * (P(0, 1) * _tmp9 + P(1, 1) * _tmp8 + P(2, 1) * _tmp7 + P(3, 1) * _tmp10) +
|
||||
_tmp9 * (P(0, 0) * _tmp9 + P(1, 0) * _tmp8 + P(2, 0) * _tmp7 + P(3, 0) * _tmp10);
|
||||
_innov_var =
|
||||
R + _tmp13 * (P(0, 3) * _tmp8 + P(1, 3) * _tmp9 + P(2, 3) * _tmp14 + P(3, 3) * _tmp13) +
|
||||
_tmp14 * (P(0, 2) * _tmp8 + P(1, 2) * _tmp9 + P(2, 2) * _tmp14 + P(3, 2) * _tmp13) +
|
||||
_tmp8 * (P(0, 0) * _tmp8 + P(1, 0) * _tmp9 + P(2, 0) * _tmp14 + P(3, 0) * _tmp13) +
|
||||
_tmp9 * (P(0, 1) * _tmp8 + P(1, 1) * _tmp9 + P(2, 1) * _tmp14 + P(3, 1) * _tmp13);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
@@ -64,10 +68,10 @@ void ComputeYaw321InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp9;
|
||||
_h(1, 0) = _tmp8;
|
||||
_h(2, 0) = _tmp7;
|
||||
_h(3, 0) = _tmp10;
|
||||
_h(0, 0) = _tmp8;
|
||||
_h(1, 0) = _tmp9;
|
||||
_h(2, 0) = _tmp14;
|
||||
_h(3, 0) = _tmp13;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+26
-22
@@ -30,33 +30,37 @@ void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& sta
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 84
|
||||
// Total ops: 75
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (11)
|
||||
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 = state(0, 0) * state(3, 0) + state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp2 = 2 * _tmp1 + epsilon * ((((_tmp1) > 0) - ((_tmp1) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp3 = std::pow(_tmp2, Scalar(2));
|
||||
const Scalar _tmp4 = 2 * _tmp0 / _tmp3;
|
||||
const Scalar _tmp5 = 2 / _tmp2;
|
||||
const Scalar _tmp6 = _tmp3 / (std::pow(_tmp0, Scalar(2)) + _tmp3);
|
||||
const Scalar _tmp7 = _tmp6 * (-_tmp4 * state(0, 0) - _tmp5 * state(3, 0));
|
||||
const Scalar _tmp8 = _tmp6 * (-_tmp4 * state(2, 0) + _tmp5 * state(1, 0));
|
||||
const Scalar _tmp9 = _tmp6 * (-_tmp4 * state(3, 0) + _tmp5 * state(0, 0));
|
||||
const Scalar _tmp10 = _tmp6 * (-_tmp4 * state(1, 0) - _tmp5 * state(2, 0));
|
||||
// Intermediate terms (15)
|
||||
const Scalar _tmp0 = 2 * state(2, 0);
|
||||
const Scalar _tmp1 =
|
||||
-2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
|
||||
const Scalar _tmp2 = 2 * state(0, 0);
|
||||
const Scalar _tmp3 = _tmp0 * state(1, 0) + _tmp2 * state(3, 0);
|
||||
const Scalar _tmp4 = _tmp3 + epsilon * ((((_tmp3) > 0) - ((_tmp3) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp5 = std::pow(_tmp4, Scalar(2));
|
||||
const Scalar _tmp6 = Scalar(1.0) / (std::pow(_tmp1, Scalar(2)) + _tmp5);
|
||||
const Scalar _tmp7 = _tmp1 * _tmp6;
|
||||
const Scalar _tmp8 = _tmp0 * _tmp7;
|
||||
const Scalar _tmp9 = 2 * _tmp7 * state(3, 0);
|
||||
const Scalar _tmp10 = _tmp1 / _tmp5;
|
||||
const Scalar _tmp11 = 4 / _tmp4;
|
||||
const Scalar _tmp12 = _tmp5 * _tmp6;
|
||||
const Scalar _tmp13 = _tmp12 * (-_tmp10 * _tmp2 - _tmp11 * state(3, 0));
|
||||
const Scalar _tmp14 = _tmp12 * (-2 * _tmp10 * state(1, 0) - _tmp11 * state(2, 0));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var =
|
||||
R - _tmp10 * (-P(0, 2) * _tmp9 - P(1, 2) * _tmp8 - P(2, 2) * _tmp10 - P(3, 2) * _tmp7) -
|
||||
_tmp7 * (-P(0, 3) * _tmp9 - P(1, 3) * _tmp8 - P(2, 3) * _tmp10 - P(3, 3) * _tmp7) -
|
||||
_tmp8 * (-P(0, 1) * _tmp9 - P(1, 1) * _tmp8 - P(2, 1) * _tmp10 - P(3, 1) * _tmp7) -
|
||||
_tmp9 * (-P(0, 0) * _tmp9 - P(1, 0) * _tmp8 - P(2, 0) * _tmp10 - P(3, 0) * _tmp7);
|
||||
R - _tmp13 * (P(0, 3) * _tmp9 + P(1, 3) * _tmp8 - P(2, 3) * _tmp14 - P(3, 3) * _tmp13) -
|
||||
_tmp14 * (P(0, 2) * _tmp9 + P(1, 2) * _tmp8 - P(2, 2) * _tmp14 - P(3, 2) * _tmp13) +
|
||||
_tmp8 * (P(0, 1) * _tmp9 + P(1, 1) * _tmp8 - P(2, 1) * _tmp14 - P(3, 1) * _tmp13) +
|
||||
_tmp9 * (P(0, 0) * _tmp9 + P(1, 0) * _tmp8 - P(2, 0) * _tmp14 - P(3, 0) * _tmp13);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
@@ -64,10 +68,10 @@ void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& sta
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = -_tmp9;
|
||||
_h(1, 0) = -_tmp8;
|
||||
_h(2, 0) = -_tmp10;
|
||||
_h(3, 0) = -_tmp7;
|
||||
_h(0, 0) = _tmp9;
|
||||
_h(1, 0) = _tmp8;
|
||||
_h(2, 0) = -_tmp14;
|
||||
_h(3, 0) = -_tmp13;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user