mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-21 13:02:25 +08:00
ekf2: use global definition of quaternion error
This commit is contained in:
bresch
Daniel Agar
@@ -279,8 +279,7 @@ void Ekf::resetQuatCov(const float yaw_noise)
|
||||
|
||||
void Ekf::resetQuatCov(const Vector3f &rot_var_ned)
|
||||
{
|
||||
matrix::SquareMatrix<float, State::quat_nominal.dof> q_cov_ned = diag(rot_var_ned);
|
||||
resetStateCovariance<State::quat_nominal>(_R_to_earth.T() * q_cov_ned * _R_to_earth);
|
||||
P.uncorrelateCovarianceSetVariance<State::quat_nominal.dof>(State::quat_nominal.idx, rot_var_ned);
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
||||
@@ -254,7 +254,7 @@ public:
|
||||
// get the diagonal elements of the covariance matrix
|
||||
matrix::Vector<float, State::size> covariances_diagonal() const { return P.diag(); }
|
||||
|
||||
matrix::Vector<float, State::quat_nominal.dof> getQuaternionVariance() const { return getStateVariance<State::quat_nominal>(); }
|
||||
matrix::Vector3f getRotVarBody() const;
|
||||
matrix::Vector3f getRotVarNed() const;
|
||||
float getYawVar() const;
|
||||
float getTiltVariance() const;
|
||||
@@ -1109,7 +1109,7 @@ private:
|
||||
#endif // CONFIG_EKF2_GRAVITY_FUSION
|
||||
|
||||
void resetQuatCov(const float yaw_noise = NAN);
|
||||
void resetQuatCov(const Vector3f &euler_noise_ned);
|
||||
void resetQuatCov(const Vector3f &rot_var_ned);
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
void resetMagCov();
|
||||
@@ -1124,9 +1124,6 @@ private:
|
||||
|
||||
void resetGyroBiasZCov();
|
||||
|
||||
// uncorrelate quaternion states from other states
|
||||
void uncorrelateQuatFromOtherStates();
|
||||
|
||||
bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t timeout_period) const
|
||||
{
|
||||
return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < _time_delayed_us);
|
||||
|
||||
@@ -552,7 +552,7 @@ void Ekf::fuse(const VectorState &K, float innovation)
|
||||
{
|
||||
// quat_nominal
|
||||
Quatf delta_quat(matrix::AxisAnglef(K.slice<State::quat_nominal.dof, 1>(State::quat_nominal.idx, 0) * (-1.f * innovation)));
|
||||
_state.quat_nominal *= delta_quat;
|
||||
_state.quat_nominal = delta_quat * _state.quat_nominal;
|
||||
_state.quat_nominal.normalize();
|
||||
_R_to_earth = Dcmf(_state.quat_nominal);
|
||||
|
||||
@@ -586,11 +586,6 @@ void Ekf::fuse(const VectorState &K, float innovation)
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
}
|
||||
|
||||
void Ekf::uncorrelateQuatFromOtherStates()
|
||||
{
|
||||
P.uncorrelateCovarianceBlock<State::quat_nominal.dof>(State::quat_nominal.idx);
|
||||
}
|
||||
|
||||
void Ekf::updateDeadReckoningStatus()
|
||||
{
|
||||
updateHorizontalDeadReckoningstatus();
|
||||
@@ -660,10 +655,16 @@ void Ekf::updateVerticalDeadReckoningStatus()
|
||||
}
|
||||
}
|
||||
|
||||
Vector3f Ekf::getRotVarNed() const
|
||||
Vector3f Ekf::getRotVarBody() const
|
||||
{
|
||||
const matrix::SquareMatrix3f rot_cov_body = getStateCovariance<State::quat_nominal>();
|
||||
return matrix::SquareMatrix<float, State::quat_nominal.dof>(_R_to_earth * rot_cov_body * _R_to_earth.T()).diag();
|
||||
return matrix::SquareMatrix3f(_R_to_earth.T() * rot_cov_body * _R_to_earth).diag();
|
||||
}
|
||||
|
||||
Vector3f Ekf::getRotVarNed() const
|
||||
{
|
||||
const matrix::SquareMatrix3f rot_cov_ned = getStateCovariance<State::quat_nominal>();
|
||||
return rot_cov_ned.diag();
|
||||
}
|
||||
|
||||
float Ekf::getYawVar() const
|
||||
@@ -707,12 +708,9 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
|
||||
// save a copy of the quaternion state for later use in calculating the amount of reset change
|
||||
const Quatf quat_before_reset = _state.quat_nominal;
|
||||
|
||||
// save a copy of covariance in NED frame to restore it after the quat reset
|
||||
Vector3f rot_var_ned_before_reset = getRotVarNed();
|
||||
|
||||
// update the yaw angle variance
|
||||
if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) {
|
||||
rot_var_ned_before_reset(2) = yaw_variance;
|
||||
P.uncorrelateCovarianceSetVariance<1>(2, yaw_variance);
|
||||
}
|
||||
|
||||
// update transformation matrix from body to world frame using the current estimate
|
||||
@@ -725,10 +723,6 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
|
||||
|
||||
// update quaternion states
|
||||
_state.quat_nominal = quat_after_reset;
|
||||
uncorrelateQuatFromOtherStates();
|
||||
|
||||
// restore covariance
|
||||
resetQuatCov(rot_var_ned_before_reset);
|
||||
|
||||
// add the reset amount to the output observer buffered data
|
||||
_output_predictor.resetQuaternion(q_error);
|
||||
|
||||
@@ -148,7 +148,7 @@ def predict_covariance(
|
||||
for key in state.keys():
|
||||
if key == "quat_nominal":
|
||||
# Create true quaternion using small angle approximation of the error rotation
|
||||
state_t["quat_nominal"] = state["quat_nominal"] * sf.Rot3(sf.Quaternion(xyz=(state_error["theta"] / 2), w=1))
|
||||
state_t["quat_nominal"] = sf.Rot3(sf.Quaternion(xyz=(state_error["theta"] / 2), w=1)) * state["quat_nominal"]
|
||||
else:
|
||||
state_t[key] = state[key] + state_error[key]
|
||||
|
||||
@@ -184,7 +184,7 @@ def predict_covariance(
|
||||
state_error_pred = Values()
|
||||
for key in state_error.keys():
|
||||
if key == "theta":
|
||||
delta_q = sf.Quaternion.from_storage(state_pred["quat_nominal"].to_storage()).conj() * sf.Quaternion.from_storage(state_t_pred["quat_nominal"].to_storage())
|
||||
delta_q = sf.Quaternion.from_storage(state_t_pred["quat_nominal"].to_storage()) * sf.Quaternion.from_storage(state_pred["quat_nominal"].to_storage()).conj()
|
||||
state_error_pred["theta"] = 2 * sf.V3(delta_q.x, delta_q.y, delta_q.z) # Use small angle approximation to obtain a simpler jacobian
|
||||
else:
|
||||
state_error_pred[key] = state_t_pred[key] - state_pred[key]
|
||||
@@ -216,6 +216,36 @@ def predict_covariance(
|
||||
|
||||
return P_new
|
||||
|
||||
def jacobian_chain_rule(expr: sf.Scalar , state: State):
|
||||
# First compute the jacobian in the parameter space
|
||||
dh_dx = sf.V1(expr).jacobian(state, tangent_space=False)
|
||||
|
||||
class MStorageTangent(sf.Matrix):
|
||||
SHAPE = (State.storage_dim(), State.tangent_dim())
|
||||
|
||||
# Then compute the jarobian mapping infinitesimal elements of the parameter space to the error state
|
||||
# Note that this jacobian only depends on the structure of the EKF
|
||||
dx_derror = MStorageTangent()
|
||||
q = sf.Quaternion.from_storage(state["quat_nominal"].to_storage())
|
||||
p = sf.Quaternion.symbolic('p')
|
||||
|
||||
pq = p * q
|
||||
qR = sf.M41(pq.to_storage()).jacobian(sf.M41(p.to_storage())) # Right quaternion product matrix
|
||||
dx_derror[0:4, 0:3] = qR / 2 * sf.M43([[1, 0, 0],
|
||||
[0, 1, 0],
|
||||
[0, 0, 1],
|
||||
[0, 0, 0]])
|
||||
|
||||
# The rest of the matrix is trivial
|
||||
for i in range(4, State.storage_dim()):
|
||||
for j in range(3, State.tangent_dim()):
|
||||
if (i == j+1):
|
||||
dx_derror[i, j] = 1
|
||||
|
||||
# Finally use the chain rule: dh/derror = dh/dx * dx/derror
|
||||
H = dh_dx * dx_derror
|
||||
return H
|
||||
|
||||
def compute_airspeed_innov_and_innov_var(
|
||||
state: VState,
|
||||
P: MTangent,
|
||||
@@ -231,7 +261,7 @@ def compute_airspeed_innov_and_innov_var(
|
||||
|
||||
innov = airspeed_pred - airspeed
|
||||
|
||||
H = sf.V1(airspeed_pred).jacobian(state)
|
||||
H = jacobian_chain_rule(airspeed_pred, state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov, innov_var)
|
||||
@@ -247,7 +277,7 @@ def compute_airspeed_h_and_k(
|
||||
wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0)
|
||||
vel_rel = state["vel"] - wind
|
||||
airspeed_pred = vel_rel.norm(epsilon=epsilon)
|
||||
H = sf.V1(airspeed_pred).jacobian(state)
|
||||
H = jacobian_chain_rule(airspeed_pred, state)
|
||||
|
||||
K = P * H.T / sf.Max(innov_var, epsilon)
|
||||
|
||||
@@ -309,7 +339,7 @@ def compute_sideslip_innov_and_innov_var(
|
||||
|
||||
innov = sideslip_pred - 0.0
|
||||
|
||||
H = sf.V1(sideslip_pred).jacobian(state)
|
||||
H = jacobian_chain_rule(sideslip_pred, state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov, innov_var)
|
||||
@@ -324,7 +354,7 @@ def compute_sideslip_h_and_k(
|
||||
state = vstate_to_state(state)
|
||||
sideslip_pred = predict_sideslip(state, epsilon);
|
||||
|
||||
H = sf.V1(sideslip_pred).jacobian(state)
|
||||
H = jacobian_chain_rule(sideslip_pred, state)
|
||||
|
||||
K = P * H.T / sf.Max(innov_var, epsilon)
|
||||
|
||||
@@ -351,11 +381,11 @@ def compute_mag_innov_innov_var_and_hx(
|
||||
innov = meas_pred - meas
|
||||
|
||||
innov_var = sf.V3()
|
||||
Hx = sf.V1(meas_pred[0]).jacobian(state)
|
||||
Hx = jacobian_chain_rule(meas_pred[0], state)
|
||||
innov_var[0] = (Hx * P * Hx.T + R)[0,0]
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state)
|
||||
Hy = jacobian_chain_rule(meas_pred[1], state)
|
||||
innov_var[1] = (Hy * P * Hy.T + R)[0,0]
|
||||
Hz = sf.V1(meas_pred[2]).jacobian(state)
|
||||
Hz = jacobian_chain_rule(meas_pred[2], state)
|
||||
innov_var[2] = (Hz * P * Hz.T + R)[0,0]
|
||||
|
||||
return (innov, innov_var, Hx.T)
|
||||
@@ -370,7 +400,7 @@ def compute_mag_y_innov_var_and_h(
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_mag_body(state);
|
||||
|
||||
H = sf.V1(meas_pred[1]).jacobian(state)
|
||||
H = jacobian_chain_rule(meas_pred[1], state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
@@ -385,7 +415,7 @@ def compute_mag_z_innov_var_and_h(
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_mag_body(state);
|
||||
|
||||
H = sf.V1(meas_pred[2]).jacobian(state)
|
||||
H = jacobian_chain_rule(meas_pred[2], state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
@@ -402,8 +432,11 @@ def compute_yaw_innov_var_and_h(
|
||||
delta_q = q * r.conj() # create a quaternion error of the measurement at the origin
|
||||
delta_meas_pred = 2 * delta_q.z # Use small angle approximation to obtain a simpler jacobian
|
||||
|
||||
H = sf.V1(delta_meas_pred).jacobian(state)
|
||||
H = jacobian_chain_rule(delta_meas_pred, state)
|
||||
H = H.subs({r.w: q.w, r.x: q.x, r.y: q.y, r.z: q.z}) # assume innovation is small
|
||||
|
||||
for i in range(State.tangent_dim()):
|
||||
H[i] = sp.factor(H[i]).subs(q.w**2 + q.x**2 + q.y**2 + q.z**2, 1) # unit norm quaternion
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
@@ -418,7 +451,7 @@ def compute_mag_declination_pred_innov_var_and_h(
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = sf.atan2(state["mag_I"][1], state["mag_I"][0], epsilon=epsilon)
|
||||
|
||||
H = sf.V1(meas_pred).jacobian(state)
|
||||
H = jacobian_chain_rule(meas_pred, state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (meas_pred, innov_var, H.T)
|
||||
@@ -450,9 +483,9 @@ def compute_flow_xy_innov_var_and_hx(
|
||||
meas_pred = predict_opt_flow(state, distance, epsilon);
|
||||
|
||||
innov_var = sf.V2()
|
||||
Hx = sf.V1(meas_pred[0]).jacobian(state)
|
||||
Hx = jacobian_chain_rule(meas_pred[0], state)
|
||||
innov_var[0] = (Hx * P * Hx.T + R)[0,0]
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state)
|
||||
Hy = jacobian_chain_rule(meas_pred[1], state)
|
||||
innov_var[1] = (Hy * P * Hy.T + R)[0,0]
|
||||
|
||||
return (innov_var, Hx.T)
|
||||
@@ -467,7 +500,7 @@ def compute_flow_y_innov_var_and_h(
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_opt_flow(state, distance, epsilon);
|
||||
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state)
|
||||
Hy = jacobian_chain_rule(meas_pred[1], state)
|
||||
innov_var = (Hy * P * Hy.T + R)[0,0]
|
||||
|
||||
return (innov_var, Hy.T)
|
||||
@@ -492,7 +525,7 @@ def compute_gnss_yaw_pred_innov_var_and_h(
|
||||
# Calculate the yaw angle from the projection
|
||||
meas_pred = sf.atan2(ant_vec_ef[1], ant_vec_ef[0], epsilon=epsilon)
|
||||
|
||||
H = sf.V1(meas_pred).jacobian(state)
|
||||
H = jacobian_chain_rule(meas_pred, state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (meas_pred, innov_var, H.T)
|
||||
@@ -529,7 +562,7 @@ def compute_drag_x_innov_var_and_h(
|
||||
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_drag(state, rho, cd, cm, epsilon)
|
||||
Hx = sf.V1(meas_pred[0]).jacobian(state)
|
||||
Hx = jacobian_chain_rule(meas_pred[0], state)
|
||||
innov_var = (Hx * P * Hx.T + R)[0,0]
|
||||
|
||||
return (innov_var, Hx.T)
|
||||
@@ -546,7 +579,7 @@ def compute_drag_y_innov_var_and_h(
|
||||
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_drag(state, rho, cd, cm, epsilon)
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state)
|
||||
Hy = jacobian_chain_rule(meas_pred[1], state)
|
||||
innov_var = (Hy * P * Hy.T + R)[0,0]
|
||||
|
||||
return (innov_var, Hy.T)
|
||||
@@ -575,7 +608,7 @@ def compute_gravity_xyz_innov_var_and_hx(
|
||||
# calculate observation jacobian (H), kalman gain (K), and innovation variance (S)
|
||||
# for each axis
|
||||
for i in range(3):
|
||||
H[i] = sf.V1(meas_pred[i]).jacobian(state)
|
||||
H[i] = jacobian_chain_rule(meas_pred[i], state)
|
||||
innov_var[i] = (H[i] * P * H[i].T + R)[0,0]
|
||||
|
||||
return (innov_var, H[0].T)
|
||||
@@ -590,7 +623,7 @@ def compute_gravity_y_innov_var_and_h(
|
||||
meas_pred = predict_gravity_direction(state)
|
||||
|
||||
# calculate observation jacobian (H), kalman gain (K), and innovation variance (S)
|
||||
H = sf.V1(meas_pred[1]).jacobian(state)
|
||||
H = jacobian_chain_rule(meas_pred[1], state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
@@ -605,7 +638,7 @@ def compute_gravity_z_innov_var_and_h(
|
||||
meas_pred = predict_gravity_direction(state)
|
||||
|
||||
# calculate observation jacobian (H), kalman gain (K), and innovation variance (S)
|
||||
H = sf.V1(meas_pred[2]).jacobian(state)
|
||||
H = jacobian_chain_rule(meas_pred[2], state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
|
||||
+120
-107
@@ -34,95 +34,108 @@ void ComputeDragXInnovVarAndH(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, 23, 1>* const Hx = nullptr) {
|
||||
// Total ops: 317
|
||||
// Total ops: 357
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (73)
|
||||
const Scalar _tmp0 = 2 * state(3, 0);
|
||||
const Scalar _tmp1 = _tmp0 * state(0, 0);
|
||||
const Scalar _tmp2 = 2 * state(2, 0);
|
||||
const Scalar _tmp3 = _tmp2 * state(1, 0);
|
||||
// Intermediate terms (79)
|
||||
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 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp7 = -2 * _tmp6;
|
||||
const Scalar _tmp8 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp9 = -2 * _tmp8;
|
||||
const Scalar _tmp10 = _tmp7 + _tmp9 + 1;
|
||||
const Scalar _tmp11 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp12 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp13 = _tmp2 * state(0, 0);
|
||||
const Scalar _tmp14 = -_tmp13;
|
||||
const Scalar _tmp15 = _tmp0 * state(1, 0);
|
||||
const Scalar _tmp16 = _tmp14 + _tmp15;
|
||||
const Scalar _tmp17 = _tmp12 * _tmp4 + _tmp16 * state(6, 0);
|
||||
const Scalar _tmp18 = _tmp10 * _tmp11 + _tmp17;
|
||||
const Scalar _tmp19 = 2 * _tmp18;
|
||||
const Scalar _tmp20 = _tmp19 * _tmp4;
|
||||
const Scalar _tmp21 = _tmp0 * state(2, 0);
|
||||
const Scalar _tmp22 = 2 * state(0, 0) * state(1, 0);
|
||||
const Scalar _tmp23 = -_tmp22;
|
||||
const Scalar _tmp24 = _tmp21 + _tmp23;
|
||||
const Scalar _tmp25 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp26 = 1 - 2 * _tmp25;
|
||||
const Scalar _tmp27 = _tmp26 + _tmp9;
|
||||
const Scalar _tmp28 = _tmp13 + _tmp15;
|
||||
const Scalar _tmp29 = _tmp11 * _tmp28 + _tmp12 * _tmp24;
|
||||
const Scalar _tmp30 = _tmp27 * state(6, 0) + _tmp29;
|
||||
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 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp11 = 2 * state(2, 0);
|
||||
const Scalar _tmp12 = _tmp11 * state(0, 0);
|
||||
const Scalar _tmp13 = _tmp2 * state(3, 0);
|
||||
const Scalar _tmp14 = -_tmp12 + _tmp13;
|
||||
const Scalar _tmp15 = _tmp10 * _tmp4 + _tmp14 * state(6, 0) + _tmp8 * _tmp9;
|
||||
const Scalar _tmp16 = 2 * _tmp15;
|
||||
const Scalar _tmp17 = _tmp16 * _tmp4;
|
||||
const Scalar _tmp18 = _tmp11 * state(3, 0);
|
||||
const Scalar _tmp19 = _tmp2 * state(0, 0);
|
||||
const Scalar _tmp20 = _tmp18 - _tmp19;
|
||||
const Scalar _tmp21 = _tmp12 + _tmp13;
|
||||
const Scalar _tmp22 = 1 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp23 = _tmp22 + _tmp7;
|
||||
const Scalar _tmp24 = _tmp10 * _tmp20 + _tmp21 * _tmp9 + _tmp23 * state(6, 0);
|
||||
const Scalar _tmp25 = 2 * _tmp24;
|
||||
const Scalar _tmp26 = _tmp20 * _tmp25;
|
||||
const Scalar _tmp27 = _tmp22 + _tmp6;
|
||||
const Scalar _tmp28 = -_tmp1 + _tmp3;
|
||||
const Scalar _tmp29 = _tmp18 + _tmp19;
|
||||
const Scalar _tmp30 = _tmp10 * _tmp27 + _tmp28 * _tmp9 + _tmp29 * state(6, 0);
|
||||
const Scalar _tmp31 = 2 * _tmp30;
|
||||
const Scalar _tmp32 = _tmp24 * _tmp31;
|
||||
const Scalar _tmp33 = _tmp26 + _tmp7;
|
||||
const Scalar _tmp34 = -_tmp1;
|
||||
const Scalar _tmp35 = _tmp3 + _tmp34;
|
||||
const Scalar _tmp36 = _tmp21 + _tmp22;
|
||||
const Scalar _tmp37 = _tmp11 * _tmp35 + _tmp36 * state(6, 0);
|
||||
const Scalar _tmp38 = _tmp12 * _tmp33 + _tmp37;
|
||||
const Scalar _tmp39 = 2 * _tmp38;
|
||||
const Scalar _tmp40 = _tmp33 * _tmp39;
|
||||
const Scalar _tmp41 = std::sqrt(Scalar(std::pow(_tmp18, Scalar(2)) + std::pow(_tmp30, Scalar(2)) +
|
||||
std::pow(_tmp38, Scalar(2)) + epsilon));
|
||||
const Scalar _tmp42 = cd * rho;
|
||||
const Scalar _tmp43 = Scalar(0.25) * _tmp18 * _tmp42 / _tmp41;
|
||||
const Scalar _tmp44 = Scalar(0.5) * _tmp41 * _tmp42;
|
||||
const Scalar _tmp45 = _tmp4 * _tmp44;
|
||||
const Scalar _tmp46 = -_tmp43 * (_tmp20 + _tmp32 + _tmp40) - _tmp45 - _tmp5;
|
||||
const Scalar _tmp47 = -_tmp25;
|
||||
const Scalar _tmp48 = _tmp47 + _tmp6;
|
||||
const Scalar _tmp49 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp50 = -_tmp49;
|
||||
const Scalar _tmp51 = _tmp50 + _tmp8;
|
||||
const Scalar _tmp52 = -_tmp3;
|
||||
const Scalar _tmp53 = -_tmp15;
|
||||
const Scalar _tmp54 = -_tmp6;
|
||||
const Scalar _tmp55 = _tmp12 * (_tmp47 + _tmp49 + _tmp54 + _tmp8) + _tmp37;
|
||||
const Scalar _tmp56 = -_tmp43 * (_tmp19 * _tmp55 + _tmp39 * (_tmp11 * (_tmp48 + _tmp51) +
|
||||
_tmp12 * (_tmp34 + _tmp52) +
|
||||
state(6, 0) * (_tmp13 + _tmp53))) -
|
||||
_tmp44 * _tmp55 - _tmp55 * cm;
|
||||
const Scalar _tmp57 = -_tmp43 * (-_tmp20 - _tmp32 - _tmp40) + _tmp45 + _tmp5;
|
||||
const Scalar _tmp58 = _tmp10 * cm;
|
||||
const Scalar _tmp59 = _tmp10 * _tmp19;
|
||||
const Scalar _tmp60 = _tmp28 * _tmp31;
|
||||
const Scalar _tmp61 = _tmp35 * _tmp39;
|
||||
const Scalar _tmp62 = _tmp10 * _tmp44;
|
||||
const Scalar _tmp63 = -_tmp43 * (-_tmp59 - _tmp60 - _tmp61) + _tmp58 + _tmp62;
|
||||
const Scalar _tmp64 = -_tmp8;
|
||||
const Scalar _tmp65 = -_tmp21;
|
||||
const Scalar _tmp66 = _tmp49 + _tmp64;
|
||||
const Scalar _tmp67 =
|
||||
_tmp43 * (_tmp31 * (_tmp11 * (_tmp1 + _tmp52) + _tmp12 * (_tmp25 + _tmp50 + _tmp6 + _tmp64) +
|
||||
state(6, 0) * (_tmp23 + _tmp65)) +
|
||||
_tmp39 * (_tmp29 + state(6, 0) * (_tmp48 + _tmp66)));
|
||||
const Scalar _tmp68 = _tmp25 + _tmp54;
|
||||
const Scalar _tmp69 =
|
||||
_tmp11 * (_tmp14 + _tmp53) + _tmp12 * (_tmp22 + _tmp65) + state(6, 0) * (_tmp51 + _tmp68);
|
||||
const Scalar _tmp70 =
|
||||
-_tmp43 * (_tmp19 * _tmp69 + _tmp31 * (_tmp11 * (_tmp66 + _tmp68) + _tmp17)) -
|
||||
_tmp44 * _tmp69 - _tmp69 * cm;
|
||||
const Scalar _tmp71 = -_tmp43 * (_tmp59 + _tmp60 + _tmp61) - _tmp58 - _tmp62;
|
||||
const Scalar _tmp72 = -_tmp16 * _tmp44 - _tmp16 * cm -
|
||||
_tmp43 * (_tmp16 * _tmp19 + _tmp27 * _tmp31 + _tmp36 * _tmp39);
|
||||
const Scalar _tmp32 = _tmp27 * _tmp31;
|
||||
const Scalar _tmp33 = std::sqrt(Scalar(std::pow(_tmp15, Scalar(2)) + std::pow(_tmp24, Scalar(2)) +
|
||||
std::pow(_tmp30, Scalar(2)) + epsilon));
|
||||
const Scalar _tmp34 = cd * rho;
|
||||
const Scalar _tmp35 = Scalar(0.25) * _tmp15 * _tmp34 / _tmp33;
|
||||
const Scalar _tmp36 = Scalar(0.5) * _tmp33 * _tmp34;
|
||||
const Scalar _tmp37 = _tmp36 * _tmp4;
|
||||
const Scalar _tmp38 = -_tmp35 * (-_tmp17 - _tmp26 - _tmp32) + _tmp37 + _tmp5;
|
||||
const Scalar _tmp39 = -_tmp35 * (_tmp17 + _tmp26 + _tmp32) - _tmp37 - _tmp5;
|
||||
const Scalar _tmp40 = _tmp8 * cm;
|
||||
const Scalar _tmp41 = _tmp16 * _tmp8;
|
||||
const Scalar _tmp42 = _tmp21 * _tmp25;
|
||||
const Scalar _tmp43 = _tmp28 * _tmp31;
|
||||
const Scalar _tmp44 = _tmp36 * _tmp8;
|
||||
const Scalar _tmp45 = -_tmp35 * (-_tmp41 - _tmp42 - _tmp43) + _tmp40 + _tmp44;
|
||||
const Scalar _tmp46 = 2 * state(3, 0);
|
||||
const Scalar _tmp47 = _tmp10 * _tmp46;
|
||||
const Scalar _tmp48 = 2 * state(6, 0);
|
||||
const Scalar _tmp49 = _tmp48 * state(2, 0);
|
||||
const Scalar _tmp50 = _tmp47 - _tmp49;
|
||||
const Scalar _tmp51 = _tmp10 * _tmp2;
|
||||
const Scalar _tmp52 = _tmp11 * _tmp9;
|
||||
const Scalar _tmp53 = _tmp46 * _tmp9;
|
||||
const Scalar _tmp54 = _tmp2 * state(6, 0);
|
||||
const Scalar _tmp55 =
|
||||
-_tmp35 * (_tmp16 * _tmp50 + _tmp25 * (-_tmp51 + _tmp52) + _tmp31 * (-_tmp53 + _tmp54)) -
|
||||
_tmp36 * _tmp50 - _tmp50 * cm;
|
||||
const Scalar _tmp56 = (Scalar(1) / Scalar(2)) * _tmp55;
|
||||
const Scalar _tmp57 = _tmp10 * _tmp11;
|
||||
const Scalar _tmp58 = _tmp48 * state(3, 0);
|
||||
const Scalar _tmp59 = _tmp57 + _tmp58;
|
||||
const Scalar _tmp60 = _tmp0 * _tmp10;
|
||||
const Scalar _tmp61 = 4 * state(6, 0);
|
||||
const Scalar _tmp62 = 4 * _tmp10;
|
||||
const Scalar _tmp63 = _tmp48 * state(0, 0);
|
||||
const Scalar _tmp64 =
|
||||
-_tmp35 * (_tmp16 * _tmp59 + _tmp25 * (_tmp53 - _tmp60 - _tmp61 * state(1, 0)) +
|
||||
_tmp31 * (_tmp52 - _tmp62 * state(1, 0) + _tmp63)) -
|
||||
_tmp36 * _tmp59 - _tmp59 * cm;
|
||||
const Scalar _tmp65 = (Scalar(1) / Scalar(2)) * state(2, 0);
|
||||
const Scalar _tmp66 = 4 * _tmp9;
|
||||
const Scalar _tmp67 = _tmp54 + _tmp60 - _tmp66 * state(3, 0);
|
||||
const Scalar _tmp68 = _tmp2 * _tmp9;
|
||||
const Scalar _tmp69 = _tmp0 * _tmp9;
|
||||
const Scalar _tmp70 = -Scalar(1) / Scalar(2) * _tmp35 *
|
||||
(_tmp16 * _tmp67 + _tmp25 * (_tmp57 + _tmp68) +
|
||||
_tmp31 * (_tmp49 - _tmp62 * state(3, 0) - _tmp69)) -
|
||||
Scalar(1) / Scalar(2) * _tmp36 * _tmp67 -
|
||||
Scalar(1) / Scalar(2) * _tmp67 * cm;
|
||||
const Scalar _tmp71 = _tmp51 - _tmp63 - _tmp66 * state(2, 0);
|
||||
const Scalar _tmp72 = -Scalar(1) / Scalar(2) * _tmp35 *
|
||||
(_tmp16 * _tmp71 + _tmp25 * (_tmp47 - _tmp61 * state(2, 0) + _tmp69) +
|
||||
_tmp31 * (_tmp58 + _tmp68)) -
|
||||
Scalar(1) / Scalar(2) * _tmp36 * _tmp71 -
|
||||
Scalar(1) / Scalar(2) * _tmp71 * cm;
|
||||
const Scalar _tmp73 =
|
||||
-_tmp56 * state(3, 0) - _tmp64 * _tmp65 + _tmp70 * state(0, 0) + _tmp72 * state(1, 0);
|
||||
const Scalar _tmp74 = (Scalar(1) / Scalar(2)) * _tmp64;
|
||||
const Scalar _tmp75 =
|
||||
-_tmp55 * _tmp65 - _tmp70 * state(1, 0) + _tmp72 * state(0, 0) + _tmp74 * state(3, 0);
|
||||
const Scalar _tmp76 =
|
||||
-_tmp56 * state(1, 0) + _tmp70 * state(2, 0) - _tmp72 * state(3, 0) + _tmp74 * state(0, 0);
|
||||
const Scalar _tmp77 = -_tmp35 * (_tmp41 + _tmp42 + _tmp43) - _tmp40 - _tmp44;
|
||||
const Scalar _tmp78 = -_tmp14 * _tmp36 - _tmp14 * cm -
|
||||
_tmp35 * (_tmp14 * _tmp16 + _tmp23 * _tmp25 + _tmp29 * _tmp31);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
@@ -130,22 +143,22 @@ void ComputeDragXInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_innov_var =
|
||||
R +
|
||||
_tmp46 * (-P(0, 4) * _tmp67 + P(1, 4) * _tmp70 + P(2, 4) * _tmp56 + P(21, 4) * _tmp63 +
|
||||
P(22, 4) * _tmp57 + P(3, 4) * _tmp71 + P(4, 4) * _tmp46 + P(5, 4) * _tmp72) +
|
||||
_tmp56 * (-P(0, 2) * _tmp67 + P(1, 2) * _tmp70 + P(2, 2) * _tmp56 + P(21, 2) * _tmp63 +
|
||||
P(22, 2) * _tmp57 + P(3, 2) * _tmp71 + P(4, 2) * _tmp46 + P(5, 2) * _tmp72) +
|
||||
_tmp57 * (-P(0, 22) * _tmp67 + P(1, 22) * _tmp70 + P(2, 22) * _tmp56 + P(21, 22) * _tmp63 +
|
||||
P(22, 22) * _tmp57 + P(3, 22) * _tmp71 + P(4, 22) * _tmp46 + P(5, 22) * _tmp72) +
|
||||
_tmp63 * (-P(0, 21) * _tmp67 + P(1, 21) * _tmp70 + P(2, 21) * _tmp56 + P(21, 21) * _tmp63 +
|
||||
P(22, 21) * _tmp57 + P(3, 21) * _tmp71 + P(4, 21) * _tmp46 + P(5, 21) * _tmp72) -
|
||||
_tmp67 * (-P(0, 0) * _tmp67 + P(1, 0) * _tmp70 + P(2, 0) * _tmp56 + P(21, 0) * _tmp63 +
|
||||
P(22, 0) * _tmp57 + P(3, 0) * _tmp71 + P(4, 0) * _tmp46 + P(5, 0) * _tmp72) +
|
||||
_tmp70 * (-P(0, 1) * _tmp67 + P(1, 1) * _tmp70 + P(2, 1) * _tmp56 + P(21, 1) * _tmp63 +
|
||||
P(22, 1) * _tmp57 + P(3, 1) * _tmp71 + P(4, 1) * _tmp46 + P(5, 1) * _tmp72) +
|
||||
_tmp71 * (-P(0, 3) * _tmp67 + P(1, 3) * _tmp70 + P(2, 3) * _tmp56 + P(21, 3) * _tmp63 +
|
||||
P(22, 3) * _tmp57 + P(3, 3) * _tmp71 + P(4, 3) * _tmp46 + P(5, 3) * _tmp72) +
|
||||
_tmp72 * (-P(0, 5) * _tmp67 + P(1, 5) * _tmp70 + P(2, 5) * _tmp56 + P(21, 5) * _tmp63 +
|
||||
P(22, 5) * _tmp57 + P(3, 5) * _tmp71 + P(4, 5) * _tmp46 + P(5, 5) * _tmp72);
|
||||
_tmp38 * (P(0, 22) * _tmp76 + P(1, 22) * _tmp75 + P(2, 22) * _tmp73 + P(21, 22) * _tmp45 +
|
||||
P(22, 22) * _tmp38 + P(3, 22) * _tmp77 + P(4, 22) * _tmp39 + P(5, 22) * _tmp78) +
|
||||
_tmp39 * (P(0, 4) * _tmp76 + P(1, 4) * _tmp75 + P(2, 4) * _tmp73 + P(21, 4) * _tmp45 +
|
||||
P(22, 4) * _tmp38 + P(3, 4) * _tmp77 + P(4, 4) * _tmp39 + P(5, 4) * _tmp78) +
|
||||
_tmp45 * (P(0, 21) * _tmp76 + P(1, 21) * _tmp75 + P(2, 21) * _tmp73 + P(21, 21) * _tmp45 +
|
||||
P(22, 21) * _tmp38 + P(3, 21) * _tmp77 + P(4, 21) * _tmp39 + P(5, 21) * _tmp78) +
|
||||
_tmp73 * (P(0, 2) * _tmp76 + P(1, 2) * _tmp75 + P(2, 2) * _tmp73 + P(21, 2) * _tmp45 +
|
||||
P(22, 2) * _tmp38 + P(3, 2) * _tmp77 + P(4, 2) * _tmp39 + P(5, 2) * _tmp78) +
|
||||
_tmp75 * (P(0, 1) * _tmp76 + P(1, 1) * _tmp75 + P(2, 1) * _tmp73 + P(21, 1) * _tmp45 +
|
||||
P(22, 1) * _tmp38 + P(3, 1) * _tmp77 + P(4, 1) * _tmp39 + P(5, 1) * _tmp78) +
|
||||
_tmp76 * (P(0, 0) * _tmp76 + P(1, 0) * _tmp75 + P(2, 0) * _tmp73 + P(21, 0) * _tmp45 +
|
||||
P(22, 0) * _tmp38 + P(3, 0) * _tmp77 + P(4, 0) * _tmp39 + P(5, 0) * _tmp78) +
|
||||
_tmp77 * (P(0, 3) * _tmp76 + P(1, 3) * _tmp75 + P(2, 3) * _tmp73 + P(21, 3) * _tmp45 +
|
||||
P(22, 3) * _tmp38 + P(3, 3) * _tmp77 + P(4, 3) * _tmp39 + P(5, 3) * _tmp78) +
|
||||
_tmp78 * (P(0, 5) * _tmp76 + P(1, 5) * _tmp75 + P(2, 5) * _tmp73 + P(21, 5) * _tmp45 +
|
||||
P(22, 5) * _tmp38 + P(3, 5) * _tmp77 + P(4, 5) * _tmp39 + P(5, 5) * _tmp78);
|
||||
}
|
||||
|
||||
if (Hx != nullptr) {
|
||||
@@ -153,14 +166,14 @@ void ComputeDragXInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_hx.setZero();
|
||||
|
||||
_hx(0, 0) = -_tmp67;
|
||||
_hx(1, 0) = _tmp70;
|
||||
_hx(2, 0) = _tmp56;
|
||||
_hx(3, 0) = _tmp71;
|
||||
_hx(4, 0) = _tmp46;
|
||||
_hx(5, 0) = _tmp72;
|
||||
_hx(21, 0) = _tmp63;
|
||||
_hx(22, 0) = _tmp57;
|
||||
_hx(0, 0) = _tmp76;
|
||||
_hx(1, 0) = _tmp75;
|
||||
_hx(2, 0) = _tmp73;
|
||||
_hx(3, 0) = _tmp77;
|
||||
_hx(4, 0) = _tmp39;
|
||||
_hx(5, 0) = _tmp78;
|
||||
_hx(21, 0) = _tmp45;
|
||||
_hx(22, 0) = _tmp38;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+121
-110
@@ -34,97 +34,108 @@ void ComputeDragYInnovVarAndH(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, 23, 1>* const Hy = nullptr) {
|
||||
// Total ops: 317
|
||||
// Total ops: 360
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (73)
|
||||
const Scalar _tmp0 = 2 * state(2, 0) * state(3, 0);
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 = _tmp1 * state(0, 0);
|
||||
const Scalar _tmp3 = _tmp0 + _tmp2;
|
||||
const Scalar _tmp4 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp5 = -2 * _tmp4;
|
||||
const Scalar _tmp6 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp7 = -2 * _tmp6;
|
||||
const Scalar _tmp8 = _tmp5 + _tmp7 + 1;
|
||||
// Intermediate terms (76)
|
||||
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 = 2 * state(0, 0);
|
||||
const Scalar _tmp11 = _tmp10 * state(3, 0);
|
||||
const Scalar _tmp12 = _tmp1 * state(2, 0);
|
||||
const Scalar _tmp13 = _tmp11 + _tmp12;
|
||||
const Scalar _tmp14 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp15 = _tmp10 * state(2, 0);
|
||||
const Scalar _tmp16 = -_tmp15;
|
||||
const Scalar _tmp17 = _tmp1 * state(3, 0);
|
||||
const Scalar _tmp18 = _tmp16 + _tmp17;
|
||||
const Scalar _tmp19 = _tmp13 * _tmp14 + _tmp18 * state(6, 0);
|
||||
const Scalar _tmp20 = _tmp19 + _tmp8 * _tmp9;
|
||||
const Scalar _tmp21 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp22 = 1 - 2 * _tmp21;
|
||||
const Scalar _tmp23 = _tmp22 + _tmp7;
|
||||
const Scalar _tmp24 = -_tmp2;
|
||||
const Scalar _tmp25 = _tmp0 + _tmp24;
|
||||
const Scalar _tmp26 = _tmp15 + _tmp17;
|
||||
const Scalar _tmp27 = _tmp14 * _tmp25 + _tmp26 * _tmp9;
|
||||
const Scalar _tmp28 = _tmp23 * state(6, 0) + _tmp27;
|
||||
const Scalar _tmp29 = _tmp22 + _tmp5;
|
||||
const Scalar _tmp30 = -_tmp11;
|
||||
const Scalar _tmp31 = _tmp12 + _tmp30;
|
||||
const Scalar _tmp32 = _tmp3 * state(6, 0) + _tmp31 * _tmp9;
|
||||
const Scalar _tmp33 = _tmp14 * _tmp29 + _tmp32;
|
||||
const Scalar _tmp34 = std::sqrt(Scalar(std::pow(_tmp20, Scalar(2)) + std::pow(_tmp28, Scalar(2)) +
|
||||
std::pow(_tmp33, Scalar(2)) + epsilon));
|
||||
const Scalar _tmp35 = cd * rho;
|
||||
const Scalar _tmp36 = Scalar(0.5) * _tmp34 * _tmp35;
|
||||
const Scalar _tmp37 = 2 * _tmp20;
|
||||
const Scalar _tmp38 = 2 * _tmp28;
|
||||
const Scalar _tmp39 = 2 * _tmp33;
|
||||
const Scalar _tmp40 = Scalar(0.25) * _tmp33 * _tmp35 / _tmp34;
|
||||
const Scalar _tmp41 =
|
||||
-_tmp3 * _tmp36 - _tmp3 * cm - _tmp40 * (_tmp18 * _tmp37 + _tmp23 * _tmp38 + _tmp3 * _tmp39);
|
||||
const Scalar _tmp42 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp43 = -_tmp42;
|
||||
const Scalar _tmp44 = -_tmp6;
|
||||
const Scalar _tmp45 = -_tmp12;
|
||||
const Scalar _tmp46 = -_tmp0;
|
||||
const Scalar _tmp47 = -_tmp21;
|
||||
const Scalar _tmp48 = _tmp4 + _tmp47;
|
||||
const Scalar _tmp49 = _tmp42 + _tmp44;
|
||||
const Scalar _tmp50 = _tmp27 + state(6, 0) * (_tmp48 + _tmp49);
|
||||
const Scalar _tmp51 =
|
||||
-_tmp36 * _tmp50 -
|
||||
_tmp40 * (_tmp38 * (_tmp14 * (_tmp21 + _tmp4 + _tmp43 + _tmp44) + _tmp9 * (_tmp11 + _tmp45) +
|
||||
state(6, 0) * (_tmp24 + _tmp46)) +
|
||||
_tmp39 * _tmp50) -
|
||||
_tmp50 * cm;
|
||||
const Scalar _tmp52 = _tmp43 + _tmp6;
|
||||
const Scalar _tmp53 = -_tmp17;
|
||||
const Scalar _tmp54 =
|
||||
_tmp14 * (_tmp30 + _tmp45) + _tmp9 * (_tmp48 + _tmp52) + state(6, 0) * (_tmp15 + _tmp53);
|
||||
const Scalar _tmp55 = -_tmp4;
|
||||
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(3, 0);
|
||||
const Scalar _tmp17 = _tmp16 * state(2, 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 = -_tmp30 - _tmp37 * (_tmp32 + _tmp34 + _tmp36) - _tmp5;
|
||||
const Scalar _tmp40 = _tmp24 * cm;
|
||||
const Scalar _tmp41 = _tmp10 * _tmp31;
|
||||
const Scalar _tmp42 = _tmp19 * _tmp33;
|
||||
const Scalar _tmp43 = _tmp24 * _tmp35;
|
||||
const Scalar _tmp44 = _tmp24 * _tmp29;
|
||||
const Scalar _tmp45 = -_tmp37 * (_tmp41 + _tmp42 + _tmp43) - _tmp40 - _tmp44;
|
||||
const Scalar _tmp46 = _tmp16 * _tmp9;
|
||||
const Scalar _tmp47 = _tmp0 * _tmp11;
|
||||
const Scalar _tmp48 = state(1, 0) * state(6, 0);
|
||||
const Scalar _tmp49 = 2 * state(2, 0);
|
||||
const Scalar _tmp50 = _tmp11 * _tmp49;
|
||||
const Scalar _tmp51 = _tmp16 * state(6, 0);
|
||||
const Scalar _tmp52 = 4 * _tmp11;
|
||||
const Scalar _tmp53 = _tmp49 * _tmp9;
|
||||
const Scalar _tmp54 = _tmp0 * state(6, 0);
|
||||
const Scalar _tmp55 = -_tmp52 * state(1, 0) + _tmp53 + _tmp54;
|
||||
const Scalar _tmp56 =
|
||||
-_tmp36 * _tmp54 -
|
||||
_tmp40 * (_tmp37 * (_tmp14 * (_tmp42 + _tmp47 + _tmp55 + _tmp6) + _tmp32) + _tmp39 * _tmp54) -
|
||||
_tmp54 * cm;
|
||||
const Scalar _tmp57 = _tmp29 * cm;
|
||||
const Scalar _tmp58 = _tmp13 * _tmp37;
|
||||
const Scalar _tmp59 = _tmp25 * _tmp38;
|
||||
const Scalar _tmp60 = _tmp29 * _tmp39;
|
||||
const Scalar _tmp61 = _tmp29 * _tmp36;
|
||||
const Scalar _tmp62 = -_tmp40 * (-_tmp58 - _tmp59 - _tmp60) + _tmp57 + _tmp61;
|
||||
const Scalar _tmp63 = _tmp21 + _tmp55;
|
||||
const Scalar _tmp64 = _tmp40 * (_tmp37 * (_tmp14 * (_tmp2 + _tmp46) + _tmp9 * (_tmp16 + _tmp53) +
|
||||
state(6, 0) * (_tmp52 + _tmp63)) +
|
||||
_tmp38 * (_tmp19 + _tmp9 * (_tmp49 + _tmp63)));
|
||||
const Scalar _tmp65 = -_tmp40 * (_tmp58 + _tmp59 + _tmp60) - _tmp57 - _tmp61;
|
||||
const Scalar _tmp66 = _tmp31 * cm;
|
||||
const Scalar _tmp67 = _tmp31 * _tmp36;
|
||||
const Scalar _tmp68 = _tmp37 * _tmp8;
|
||||
const Scalar _tmp69 = _tmp26 * _tmp38;
|
||||
const Scalar _tmp70 = _tmp31 * _tmp39;
|
||||
const Scalar _tmp71 = -_tmp40 * (_tmp68 + _tmp69 + _tmp70) - _tmp66 - _tmp67;
|
||||
const Scalar _tmp72 = -_tmp40 * (-_tmp68 - _tmp69 - _tmp70) + _tmp66 + _tmp67;
|
||||
-Scalar(1) / Scalar(2) * _tmp29 * _tmp55 -
|
||||
Scalar(1) / Scalar(2) * _tmp37 *
|
||||
(_tmp31 * (_tmp50 + _tmp51) + _tmp33 * (_tmp46 - _tmp47 - 4 * _tmp48) + _tmp35 * _tmp55) -
|
||||
Scalar(1) / Scalar(2) * _tmp55 * cm;
|
||||
const Scalar _tmp57 = 2 * _tmp48;
|
||||
const Scalar _tmp58 = -_tmp46 + _tmp57;
|
||||
const Scalar _tmp59 = _tmp11 * _tmp2;
|
||||
const Scalar _tmp60 = _tmp11 * _tmp16;
|
||||
const Scalar _tmp61 = state(2, 0) * state(6, 0);
|
||||
const Scalar _tmp62 = 2 * _tmp61;
|
||||
const Scalar _tmp63 =
|
||||
-Scalar(1) / Scalar(2) * _tmp29 * _tmp58 -
|
||||
Scalar(1) / Scalar(2) * _tmp37 *
|
||||
(_tmp31 * (_tmp60 - _tmp62) + _tmp33 * (_tmp53 - _tmp59) + _tmp35 * _tmp58) -
|
||||
Scalar(1) / Scalar(2) * _tmp58 * cm;
|
||||
const Scalar _tmp64 = _tmp2 * _tmp9;
|
||||
const Scalar _tmp65 = _tmp51 + _tmp64;
|
||||
const Scalar _tmp66 = _tmp0 * _tmp9;
|
||||
const Scalar _tmp67 = 4 * _tmp9;
|
||||
const Scalar _tmp68 = -Scalar(1) / Scalar(2) * _tmp29 * _tmp65 -
|
||||
Scalar(1) / Scalar(2) * _tmp37 *
|
||||
(_tmp31 * (-_tmp54 + _tmp59 - _tmp67 * state(2, 0)) +
|
||||
_tmp33 * (_tmp60 - 4 * _tmp61 + _tmp66) + _tmp35 * _tmp65) -
|
||||
Scalar(1) / Scalar(2) * _tmp65 * cm;
|
||||
const Scalar _tmp69 = -_tmp52 * state(3, 0) + _tmp62 - _tmp66;
|
||||
const Scalar _tmp70 = -Scalar(1) / Scalar(2) * _tmp29 * _tmp69 -
|
||||
Scalar(1) / Scalar(2) * _tmp37 *
|
||||
(_tmp31 * (_tmp47 + _tmp57 - _tmp67 * state(3, 0)) +
|
||||
_tmp33 * (_tmp50 + _tmp64) + _tmp35 * _tmp69) -
|
||||
Scalar(1) / Scalar(2) * _tmp69 * cm;
|
||||
const Scalar _tmp71 =
|
||||
-_tmp56 * state(2, 0) - _tmp63 * state(3, 0) + _tmp68 * state(1, 0) + _tmp70 * state(0, 0);
|
||||
const Scalar _tmp72 =
|
||||
_tmp56 * state(3, 0) - _tmp63 * state(2, 0) + _tmp68 * state(0, 0) - _tmp70 * state(1, 0);
|
||||
const Scalar _tmp73 =
|
||||
_tmp56 * state(0, 0) - _tmp63 * state(1, 0) - _tmp68 * state(3, 0) + _tmp70 * state(2, 0);
|
||||
const Scalar _tmp74 = -_tmp37 * (-_tmp41 - _tmp42 - _tmp43) + _tmp40 + _tmp44;
|
||||
const Scalar _tmp75 = -_tmp25 * _tmp29 - _tmp25 * cm -
|
||||
_tmp37 * (_tmp14 * _tmp31 + _tmp22 * _tmp33 + _tmp25 * _tmp35);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
@@ -132,22 +143,22 @@ void ComputeDragYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_innov_var =
|
||||
R +
|
||||
_tmp41 * (P(0, 5) * _tmp51 - P(1, 5) * _tmp64 + P(2, 5) * _tmp56 + P(21, 5) * _tmp72 +
|
||||
P(22, 5) * _tmp62 + P(3, 5) * _tmp71 + P(4, 5) * _tmp65 + P(5, 5) * _tmp41) +
|
||||
_tmp51 * (P(0, 0) * _tmp51 - P(1, 0) * _tmp64 + P(2, 0) * _tmp56 + P(21, 0) * _tmp72 +
|
||||
P(22, 0) * _tmp62 + P(3, 0) * _tmp71 + P(4, 0) * _tmp65 + P(5, 0) * _tmp41) +
|
||||
_tmp56 * (P(0, 2) * _tmp51 - P(1, 2) * _tmp64 + P(2, 2) * _tmp56 + P(21, 2) * _tmp72 +
|
||||
P(22, 2) * _tmp62 + P(3, 2) * _tmp71 + P(4, 2) * _tmp65 + P(5, 2) * _tmp41) +
|
||||
_tmp62 * (P(0, 22) * _tmp51 - P(1, 22) * _tmp64 + P(2, 22) * _tmp56 + P(21, 22) * _tmp72 +
|
||||
P(22, 22) * _tmp62 + P(3, 22) * _tmp71 + P(4, 22) * _tmp65 + P(5, 22) * _tmp41) -
|
||||
_tmp64 * (P(0, 1) * _tmp51 - P(1, 1) * _tmp64 + P(2, 1) * _tmp56 + P(21, 1) * _tmp72 +
|
||||
P(22, 1) * _tmp62 + P(3, 1) * _tmp71 + P(4, 1) * _tmp65 + P(5, 1) * _tmp41) +
|
||||
_tmp65 * (P(0, 4) * _tmp51 - P(1, 4) * _tmp64 + P(2, 4) * _tmp56 + P(21, 4) * _tmp72 +
|
||||
P(22, 4) * _tmp62 + P(3, 4) * _tmp71 + P(4, 4) * _tmp65 + P(5, 4) * _tmp41) +
|
||||
_tmp71 * (P(0, 3) * _tmp51 - P(1, 3) * _tmp64 + P(2, 3) * _tmp56 + P(21, 3) * _tmp72 +
|
||||
P(22, 3) * _tmp62 + P(3, 3) * _tmp71 + P(4, 3) * _tmp65 + P(5, 3) * _tmp41) +
|
||||
_tmp72 * (P(0, 21) * _tmp51 - P(1, 21) * _tmp64 + P(2, 21) * _tmp56 + P(21, 21) * _tmp72 +
|
||||
P(22, 21) * _tmp62 + P(3, 21) * _tmp71 + P(4, 21) * _tmp65 + P(5, 21) * _tmp41);
|
||||
_tmp38 * (P(0, 21) * _tmp73 + P(1, 21) * _tmp72 + P(2, 21) * _tmp71 + P(21, 21) * _tmp38 +
|
||||
P(22, 21) * _tmp74 + P(3, 21) * _tmp39 + P(4, 21) * _tmp45 + P(5, 21) * _tmp75) +
|
||||
_tmp39 * (P(0, 3) * _tmp73 + P(1, 3) * _tmp72 + P(2, 3) * _tmp71 + P(21, 3) * _tmp38 +
|
||||
P(22, 3) * _tmp74 + P(3, 3) * _tmp39 + P(4, 3) * _tmp45 + P(5, 3) * _tmp75) +
|
||||
_tmp45 * (P(0, 4) * _tmp73 + P(1, 4) * _tmp72 + P(2, 4) * _tmp71 + P(21, 4) * _tmp38 +
|
||||
P(22, 4) * _tmp74 + P(3, 4) * _tmp39 + P(4, 4) * _tmp45 + P(5, 4) * _tmp75) +
|
||||
_tmp71 * (P(0, 2) * _tmp73 + P(1, 2) * _tmp72 + P(2, 2) * _tmp71 + P(21, 2) * _tmp38 +
|
||||
P(22, 2) * _tmp74 + P(3, 2) * _tmp39 + P(4, 2) * _tmp45 + P(5, 2) * _tmp75) +
|
||||
_tmp72 * (P(0, 1) * _tmp73 + P(1, 1) * _tmp72 + P(2, 1) * _tmp71 + P(21, 1) * _tmp38 +
|
||||
P(22, 1) * _tmp74 + P(3, 1) * _tmp39 + P(4, 1) * _tmp45 + P(5, 1) * _tmp75) +
|
||||
_tmp73 * (P(0, 0) * _tmp73 + P(1, 0) * _tmp72 + P(2, 0) * _tmp71 + P(21, 0) * _tmp38 +
|
||||
P(22, 0) * _tmp74 + P(3, 0) * _tmp39 + P(4, 0) * _tmp45 + P(5, 0) * _tmp75) +
|
||||
_tmp74 * (P(0, 22) * _tmp73 + P(1, 22) * _tmp72 + P(2, 22) * _tmp71 + P(21, 22) * _tmp38 +
|
||||
P(22, 22) * _tmp74 + P(3, 22) * _tmp39 + P(4, 22) * _tmp45 + P(5, 22) * _tmp75) +
|
||||
_tmp75 * (P(0, 5) * _tmp73 + P(1, 5) * _tmp72 + P(2, 5) * _tmp71 + P(21, 5) * _tmp38 +
|
||||
P(22, 5) * _tmp74 + P(3, 5) * _tmp39 + P(4, 5) * _tmp45 + P(5, 5) * _tmp75);
|
||||
}
|
||||
|
||||
if (Hy != nullptr) {
|
||||
@@ -155,14 +166,14 @@ void ComputeDragYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_hy.setZero();
|
||||
|
||||
_hy(0, 0) = _tmp51;
|
||||
_hy(1, 0) = -_tmp64;
|
||||
_hy(2, 0) = _tmp56;
|
||||
_hy(3, 0) = _tmp71;
|
||||
_hy(4, 0) = _tmp65;
|
||||
_hy(5, 0) = _tmp41;
|
||||
_hy(21, 0) = _tmp72;
|
||||
_hy(22, 0) = _tmp62;
|
||||
_hy(0, 0) = _tmp73;
|
||||
_hy(1, 0) = _tmp72;
|
||||
_hy(2, 0) = _tmp71;
|
||||
_hy(3, 0) = _tmp39;
|
||||
_hy(4, 0) = _tmp45;
|
||||
_hy(5, 0) = _tmp75;
|
||||
_hy(21, 0) = _tmp38;
|
||||
_hy(22, 0) = _tmp74;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+75
-63
@@ -32,77 +32,88 @@ 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, 23, 1>* const H = nullptr) {
|
||||
// Total ops: 196
|
||||
// Total ops: 275
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (33)
|
||||
const Scalar _tmp0 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp2 = 1 - 2 * _tmp1;
|
||||
const Scalar _tmp3 =
|
||||
// Intermediate terms (42)
|
||||
const Scalar _tmp0 = 2 * state(4, 0);
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 = _tmp1 * state(6, 0);
|
||||
const Scalar _tmp3 = -_tmp0 * state(3, 0) + _tmp2;
|
||||
const Scalar _tmp4 =
|
||||
Scalar(1.0) /
|
||||
(distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
|
||||
const Scalar _tmp4 = _tmp3 * (-2 * _tmp0 + _tmp2);
|
||||
const Scalar _tmp5 = 2 * state(2, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(0, 0);
|
||||
const Scalar _tmp7 = 2 * state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp8 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp5 = (Scalar(1) / Scalar(2)) * _tmp4;
|
||||
const Scalar _tmp6 = _tmp5 * state(2, 0);
|
||||
const Scalar _tmp7 = 2 * state(3, 0) * state(6, 0);
|
||||
const Scalar _tmp8 = _tmp5 * (_tmp0 * state(1, 0) + _tmp7);
|
||||
const Scalar _tmp9 = 2 * state(0, 0);
|
||||
const Scalar _tmp10 = _tmp9 * state(1, 0);
|
||||
const Scalar _tmp11 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp12 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp13 = -_tmp0;
|
||||
const Scalar _tmp14 = _tmp12 + _tmp13;
|
||||
const Scalar _tmp15 = _tmp3 * (state(4, 0) * (_tmp6 + _tmp7) + state(5, 0) * (-_tmp10 + _tmp8) +
|
||||
state(6, 0) * (_tmp1 - _tmp11 + _tmp14));
|
||||
const Scalar _tmp16 = _tmp9 * state(3, 0);
|
||||
const Scalar _tmp17 = -_tmp16;
|
||||
const Scalar _tmp18 = _tmp5 * state(1, 0);
|
||||
const Scalar _tmp19 = _tmp17 + _tmp18;
|
||||
const Scalar _tmp20 = _tmp19 * _tmp3;
|
||||
const Scalar _tmp21 = _tmp10 + _tmp8;
|
||||
const Scalar _tmp22 = _tmp21 * _tmp3;
|
||||
const Scalar _tmp23 = -_tmp7;
|
||||
const Scalar _tmp24 = -_tmp12;
|
||||
const Scalar _tmp25 = _tmp3 * (state(4, 0) * (_tmp1 + _tmp11 + _tmp13 + _tmp24) +
|
||||
state(5, 0) * (_tmp17 - _tmp18) + state(6, 0) * (_tmp23 + _tmp6));
|
||||
const Scalar _tmp26 = _tmp3 * (-2 * _tmp11 + _tmp2);
|
||||
const Scalar _tmp27 = -_tmp6;
|
||||
const Scalar _tmp28 = -_tmp1 + _tmp11;
|
||||
const Scalar _tmp29 = _tmp3 * (state(4, 0) * (_tmp23 + _tmp27) + state(5, 0) * (_tmp10 - _tmp8) +
|
||||
state(6, 0) * (_tmp0 + _tmp24 + _tmp28));
|
||||
const Scalar _tmp30 =
|
||||
_tmp3 * (_tmp19 * state(4, 0) + _tmp21 * state(6, 0) + state(5, 0) * (_tmp14 + _tmp28));
|
||||
const Scalar _tmp31 = _tmp3 * (_tmp16 + _tmp18);
|
||||
const Scalar _tmp32 = _tmp3 * (_tmp27 + _tmp7);
|
||||
const Scalar _tmp10 = state(3, 0) * state(5, 0);
|
||||
const Scalar _tmp11 = 2 * state(2, 0);
|
||||
const Scalar _tmp12 = _tmp11 * state(6, 0);
|
||||
const Scalar _tmp13 = -4 * _tmp10 + _tmp12 - _tmp9 * state(4, 0);
|
||||
const Scalar _tmp14 = _tmp5 * state(1, 0);
|
||||
const Scalar _tmp15 = _tmp9 * state(6, 0);
|
||||
const Scalar _tmp16 = _tmp0 * state(2, 0) + _tmp15 - 4 * state(1, 0) * state(5, 0);
|
||||
const Scalar _tmp17 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp18 = -_tmp13 * _tmp14 + _tmp16 * _tmp17 - _tmp3 * _tmp6 + _tmp8 * state(0, 0);
|
||||
const Scalar _tmp19 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp20 = _tmp4 * (_tmp19 - 2 * std::pow(state(1, 0), Scalar(2)));
|
||||
const Scalar _tmp21 = _tmp3 * _tmp5;
|
||||
const Scalar _tmp22 = _tmp5 * state(0, 0);
|
||||
const Scalar _tmp23 =
|
||||
_tmp13 * _tmp6 + _tmp16 * _tmp22 - _tmp21 * state(1, 0) - _tmp8 * state(3, 0);
|
||||
const Scalar _tmp24 =
|
||||
_tmp13 * _tmp22 - _tmp16 * _tmp6 - _tmp21 * state(3, 0) + _tmp8 * state(1, 0);
|
||||
const Scalar _tmp25 = _tmp9 * state(3, 0);
|
||||
const Scalar _tmp26 = _tmp1 * state(2, 0);
|
||||
const Scalar _tmp27 = _tmp4 * (-_tmp25 + _tmp26);
|
||||
const Scalar _tmp28 = _tmp4 * (_tmp11 * state(3, 0) + _tmp9 * state(1, 0));
|
||||
const Scalar _tmp29 = _tmp4 * (_tmp19 - 2 * std::pow(state(2, 0), Scalar(2)));
|
||||
const Scalar _tmp30 = 4 * state(4, 0);
|
||||
const Scalar _tmp31 = _tmp2 - _tmp30 * state(3, 0) + _tmp9 * state(5, 0);
|
||||
const Scalar _tmp32 = 2 * state(5, 0);
|
||||
const Scalar _tmp33 = -_tmp15 - _tmp30 * state(2, 0) + _tmp32 * state(1, 0);
|
||||
const Scalar _tmp34 = _tmp32 * state(2, 0) + _tmp7;
|
||||
const Scalar _tmp35 = 2 * _tmp10 - _tmp12;
|
||||
const Scalar _tmp36 = _tmp35 * _tmp5;
|
||||
const Scalar _tmp37 = _tmp17 * _tmp33 - _tmp22 * _tmp34 - _tmp31 * _tmp6 + _tmp36 * state(1, 0);
|
||||
const Scalar _tmp38 = -_tmp14 * _tmp33 - _tmp22 * _tmp31 + _tmp34 * _tmp6 + _tmp36 * state(3, 0);
|
||||
const Scalar _tmp39 = _tmp14 * _tmp31 - _tmp17 * _tmp34 - _tmp22 * _tmp33 + _tmp35 * _tmp6;
|
||||
const Scalar _tmp40 = _tmp4 * (_tmp25 + _tmp26);
|
||||
const Scalar _tmp41 = _tmp4 * (_tmp1 * state(3, 0) - _tmp9 * state(2, 0));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
matrix::Matrix<Scalar, 2, 1>& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var(0, 0) = R +
|
||||
_tmp15 * (P(0, 0) * _tmp15 + P(2, 0) * _tmp25 + P(3, 0) * _tmp20 +
|
||||
P(4, 0) * _tmp4 + P(5, 0) * _tmp22) +
|
||||
_tmp20 * (P(0, 3) * _tmp15 + P(2, 3) * _tmp25 + P(3, 3) * _tmp20 +
|
||||
P(4, 3) * _tmp4 + P(5, 3) * _tmp22) +
|
||||
_tmp22 * (P(0, 5) * _tmp15 + P(2, 5) * _tmp25 + P(3, 5) * _tmp20 +
|
||||
P(4, 5) * _tmp4 + P(5, 5) * _tmp22) +
|
||||
_tmp25 * (P(0, 2) * _tmp15 + P(2, 2) * _tmp25 + P(3, 2) * _tmp20 +
|
||||
P(4, 2) * _tmp4 + P(5, 2) * _tmp22) +
|
||||
_tmp4 * (P(0, 4) * _tmp15 + P(2, 4) * _tmp25 + P(3, 4) * _tmp20 +
|
||||
P(4, 4) * _tmp4 + P(5, 4) * _tmp22);
|
||||
_tmp18 * (P(0, 1) * _tmp23 + P(1, 1) * _tmp18 + P(2, 1) * _tmp24 +
|
||||
P(3, 1) * _tmp27 + P(4, 1) * _tmp20 + P(5, 1) * _tmp28) +
|
||||
_tmp20 * (P(0, 4) * _tmp23 + P(1, 4) * _tmp18 + P(2, 4) * _tmp24 +
|
||||
P(3, 4) * _tmp27 + P(4, 4) * _tmp20 + P(5, 4) * _tmp28) +
|
||||
_tmp23 * (P(0, 0) * _tmp23 + P(1, 0) * _tmp18 + P(2, 0) * _tmp24 +
|
||||
P(3, 0) * _tmp27 + P(4, 0) * _tmp20 + P(5, 0) * _tmp28) +
|
||||
_tmp24 * (P(0, 2) * _tmp23 + P(1, 2) * _tmp18 + P(2, 2) * _tmp24 +
|
||||
P(3, 2) * _tmp27 + P(4, 2) * _tmp20 + P(5, 2) * _tmp28) +
|
||||
_tmp27 * (P(0, 3) * _tmp23 + P(1, 3) * _tmp18 + P(2, 3) * _tmp24 +
|
||||
P(3, 3) * _tmp27 + P(4, 3) * _tmp20 + P(5, 3) * _tmp28) +
|
||||
_tmp28 * (P(0, 5) * _tmp23 + P(1, 5) * _tmp18 + P(2, 5) * _tmp24 +
|
||||
P(3, 5) * _tmp27 + P(4, 5) * _tmp20 + P(5, 5) * _tmp28);
|
||||
_innov_var(1, 0) = R -
|
||||
_tmp26 * (-P(1, 3) * _tmp29 - P(2, 3) * _tmp30 - P(3, 3) * _tmp26 -
|
||||
P(4, 3) * _tmp31 - P(5, 3) * _tmp32) -
|
||||
_tmp29 * (-P(1, 1) * _tmp29 - P(2, 1) * _tmp30 - P(3, 1) * _tmp26 -
|
||||
P(4, 1) * _tmp31 - P(5, 1) * _tmp32) -
|
||||
_tmp30 * (-P(1, 2) * _tmp29 - P(2, 2) * _tmp30 - P(3, 2) * _tmp26 -
|
||||
P(4, 2) * _tmp31 - P(5, 2) * _tmp32) -
|
||||
_tmp31 * (-P(1, 4) * _tmp29 - P(2, 4) * _tmp30 - P(3, 4) * _tmp26 -
|
||||
P(4, 4) * _tmp31 - P(5, 4) * _tmp32) -
|
||||
_tmp32 * (-P(1, 5) * _tmp29 - P(2, 5) * _tmp30 - P(3, 5) * _tmp26 -
|
||||
P(4, 5) * _tmp31 - P(5, 5) * _tmp32);
|
||||
_tmp29 * (P(0, 3) * _tmp37 + P(1, 3) * _tmp39 + P(2, 3) * _tmp38 -
|
||||
P(3, 3) * _tmp29 - P(4, 3) * _tmp40 - P(5, 3) * _tmp41) +
|
||||
_tmp37 * (P(0, 0) * _tmp37 + P(1, 0) * _tmp39 + P(2, 0) * _tmp38 -
|
||||
P(3, 0) * _tmp29 - P(4, 0) * _tmp40 - P(5, 0) * _tmp41) +
|
||||
_tmp38 * (P(0, 2) * _tmp37 + P(1, 2) * _tmp39 + P(2, 2) * _tmp38 -
|
||||
P(3, 2) * _tmp29 - P(4, 2) * _tmp40 - P(5, 2) * _tmp41) +
|
||||
_tmp39 * (P(0, 1) * _tmp37 + P(1, 1) * _tmp39 + P(2, 1) * _tmp38 -
|
||||
P(3, 1) * _tmp29 - P(4, 1) * _tmp40 - P(5, 1) * _tmp41) -
|
||||
_tmp40 * (P(0, 4) * _tmp37 + P(1, 4) * _tmp39 + P(2, 4) * _tmp38 -
|
||||
P(3, 4) * _tmp29 - P(4, 4) * _tmp40 - P(5, 4) * _tmp41) -
|
||||
_tmp41 * (P(0, 5) * _tmp37 + P(1, 5) * _tmp39 + P(2, 5) * _tmp38 -
|
||||
P(3, 5) * _tmp29 - P(4, 5) * _tmp40 - P(5, 5) * _tmp41);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
@@ -110,11 +121,12 @@ void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp15;
|
||||
_h(2, 0) = _tmp25;
|
||||
_h(3, 0) = _tmp20;
|
||||
_h(4, 0) = _tmp4;
|
||||
_h(5, 0) = _tmp22;
|
||||
_h(0, 0) = _tmp23;
|
||||
_h(1, 0) = _tmp18;
|
||||
_h(2, 0) = _tmp24;
|
||||
_h(3, 0) = _tmp27;
|
||||
_h(4, 0) = _tmp20;
|
||||
_h(5, 0) = _tmp28;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+44
-38
@@ -32,51 +32,56 @@ void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const Scalar R, const Scalar epsilon,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
// Total ops: 116
|
||||
// Total ops: 151
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (19)
|
||||
const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp2 =
|
||||
// Intermediate terms (21)
|
||||
const Scalar _tmp0 =
|
||||
Scalar(1.0) /
|
||||
(distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
|
||||
const Scalar _tmp3 = _tmp2 * (-2 * _tmp0 - 2 * _tmp1 + 1);
|
||||
const Scalar _tmp4 = -2 * state(0, 0) * state(2, 0);
|
||||
const Scalar _tmp5 = 2 * state(3, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(1, 0);
|
||||
const Scalar _tmp7 = _tmp5 * state(2, 0);
|
||||
const Scalar _tmp1 =
|
||||
_tmp0 * (-2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1);
|
||||
const Scalar _tmp2 = 4 * state(4, 0);
|
||||
const Scalar _tmp3 = 2 * state(0, 0);
|
||||
const Scalar _tmp4 = 2 * state(6, 0);
|
||||
const Scalar _tmp5 = -_tmp2 * state(3, 0) + _tmp3 * state(5, 0) + _tmp4 * state(1, 0);
|
||||
const Scalar _tmp6 = (Scalar(1) / Scalar(2)) * _tmp0;
|
||||
const Scalar _tmp7 = _tmp5 * _tmp6;
|
||||
const Scalar _tmp8 = 2 * state(1, 0);
|
||||
const Scalar _tmp9 = _tmp8 * state(0, 0);
|
||||
const Scalar _tmp10 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp11 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp12 = -_tmp0 + _tmp1;
|
||||
const Scalar _tmp13 = _tmp2 * (state(4, 0) * (_tmp4 - _tmp6) + state(5, 0) * (-_tmp7 + _tmp9) +
|
||||
state(6, 0) * (-_tmp10 + _tmp11 + _tmp12));
|
||||
const Scalar _tmp14 = _tmp5 * state(0, 0);
|
||||
const Scalar _tmp15 = _tmp8 * state(2, 0);
|
||||
const Scalar _tmp16 =
|
||||
_tmp2 * (state(4, 0) * (-_tmp14 + _tmp15) + state(5, 0) * (_tmp10 - _tmp11 + _tmp12) +
|
||||
state(6, 0) * (_tmp7 + _tmp9));
|
||||
const Scalar _tmp17 = _tmp2 * (_tmp14 + _tmp15);
|
||||
const Scalar _tmp18 = _tmp2 * (_tmp4 + _tmp6);
|
||||
const Scalar _tmp9 = -_tmp2 * state(2, 0) - _tmp4 * state(0, 0) + _tmp8 * state(5, 0);
|
||||
const Scalar _tmp10 = _tmp6 * _tmp9;
|
||||
const Scalar _tmp11 = 2 * state(5, 0);
|
||||
const Scalar _tmp12 = _tmp6 * (_tmp11 * state(2, 0) + _tmp4 * state(3, 0));
|
||||
const Scalar _tmp13 = _tmp11 * state(3, 0) - _tmp4 * state(2, 0);
|
||||
const Scalar _tmp14 = _tmp6 * state(1, 0);
|
||||
const Scalar _tmp15 =
|
||||
_tmp10 * state(3, 0) - _tmp12 * state(0, 0) + _tmp13 * _tmp14 - _tmp7 * state(2, 0);
|
||||
const Scalar _tmp16 = _tmp13 * _tmp6;
|
||||
const Scalar _tmp17 =
|
||||
_tmp12 * state(2, 0) - _tmp14 * _tmp9 + _tmp16 * state(3, 0) - _tmp7 * state(0, 0);
|
||||
const Scalar _tmp18 =
|
||||
-_tmp10 * state(0, 0) - _tmp12 * state(3, 0) + _tmp14 * _tmp5 + _tmp16 * state(2, 0);
|
||||
const Scalar _tmp19 = _tmp0 * (_tmp3 * state(3, 0) + _tmp8 * state(2, 0));
|
||||
const Scalar _tmp20 = _tmp0 * (-_tmp3 * state(2, 0) + _tmp8 * state(3, 0));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R -
|
||||
_tmp13 * (-P(1, 1) * _tmp13 - P(2, 1) * _tmp16 - P(3, 1) * _tmp3 -
|
||||
P(4, 1) * _tmp17 - P(5, 1) * _tmp18) -
|
||||
_tmp16 * (-P(1, 2) * _tmp13 - P(2, 2) * _tmp16 - P(3, 2) * _tmp3 -
|
||||
P(4, 2) * _tmp17 - P(5, 2) * _tmp18) -
|
||||
_tmp17 * (-P(1, 4) * _tmp13 - P(2, 4) * _tmp16 - P(3, 4) * _tmp3 -
|
||||
P(4, 4) * _tmp17 - P(5, 4) * _tmp18) -
|
||||
_tmp18 * (-P(1, 5) * _tmp13 - P(2, 5) * _tmp16 - P(3, 5) * _tmp3 -
|
||||
P(4, 5) * _tmp17 - P(5, 5) * _tmp18) -
|
||||
_tmp3 * (-P(1, 3) * _tmp13 - P(2, 3) * _tmp16 - P(3, 3) * _tmp3 -
|
||||
P(4, 3) * _tmp17 - P(5, 3) * _tmp18);
|
||||
_tmp1 * (P(0, 3) * _tmp15 + P(1, 3) * _tmp18 + P(2, 3) * _tmp17 - P(3, 3) * _tmp1 -
|
||||
P(4, 3) * _tmp19 - P(5, 3) * _tmp20) +
|
||||
_tmp15 * (P(0, 0) * _tmp15 + P(1, 0) * _tmp18 + P(2, 0) * _tmp17 -
|
||||
P(3, 0) * _tmp1 - P(4, 0) * _tmp19 - P(5, 0) * _tmp20) +
|
||||
_tmp17 * (P(0, 2) * _tmp15 + P(1, 2) * _tmp18 + P(2, 2) * _tmp17 -
|
||||
P(3, 2) * _tmp1 - P(4, 2) * _tmp19 - P(5, 2) * _tmp20) +
|
||||
_tmp18 * (P(0, 1) * _tmp15 + P(1, 1) * _tmp18 + P(2, 1) * _tmp17 -
|
||||
P(3, 1) * _tmp1 - P(4, 1) * _tmp19 - P(5, 1) * _tmp20) -
|
||||
_tmp19 * (P(0, 4) * _tmp15 + P(1, 4) * _tmp18 + P(2, 4) * _tmp17 -
|
||||
P(3, 4) * _tmp1 - P(4, 4) * _tmp19 - P(5, 4) * _tmp20) -
|
||||
_tmp20 * (P(0, 5) * _tmp15 + P(1, 5) * _tmp18 + P(2, 5) * _tmp17 -
|
||||
P(3, 5) * _tmp1 - P(4, 5) * _tmp19 - P(5, 5) * _tmp20);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
@@ -84,11 +89,12 @@ void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(1, 0) = -_tmp13;
|
||||
_h(2, 0) = -_tmp16;
|
||||
_h(3, 0) = -_tmp3;
|
||||
_h(4, 0) = -_tmp17;
|
||||
_h(5, 0) = -_tmp18;
|
||||
_h(0, 0) = _tmp15;
|
||||
_h(1, 0) = _tmp18;
|
||||
_h(2, 0) = _tmp17;
|
||||
_h(3, 0) = -_tmp1;
|
||||
_h(4, 0) = -_tmp19;
|
||||
_h(5, 0) = -_tmp20;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+45
-40
@@ -34,57 +34,62 @@ 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, 23, 1>* const H = nullptr) {
|
||||
// Total ops: 95
|
||||
// Total ops: 114
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (28)
|
||||
const Scalar _tmp0 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp2 = 1 - 2 * _tmp1;
|
||||
const Scalar _tmp3 = std::sin(antenna_yaw_offset);
|
||||
const Scalar _tmp4 = 2 * state(0, 0);
|
||||
const Scalar _tmp5 = _tmp4 * state(3, 0);
|
||||
const Scalar _tmp6 = 2 * state(2, 0);
|
||||
const Scalar _tmp7 = _tmp6 * state(1, 0);
|
||||
const Scalar _tmp8 = std::cos(antenna_yaw_offset);
|
||||
const Scalar _tmp9 = _tmp3 * (-2 * _tmp0 + _tmp2) + _tmp8 * (_tmp5 + _tmp7);
|
||||
const Scalar _tmp10 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp11 = -_tmp5;
|
||||
const Scalar _tmp12 = _tmp11 + _tmp7;
|
||||
const Scalar _tmp13 = _tmp12 * _tmp3 + _tmp8 * (-2 * _tmp10 + _tmp2);
|
||||
const Scalar _tmp14 = _tmp13 + epsilon * ((((_tmp13) > 0) - ((_tmp13) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp15 = _tmp6 * state(0, 0);
|
||||
const Scalar _tmp16 = 2 * state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp17 = std::pow(_tmp14, Scalar(2));
|
||||
const Scalar _tmp18 = _tmp9 / _tmp17;
|
||||
const Scalar _tmp19 = _tmp6 * state(3, 0);
|
||||
const Scalar _tmp20 = _tmp4 * state(1, 0);
|
||||
const Scalar _tmp21 = Scalar(1.0) / (_tmp14);
|
||||
const Scalar _tmp22 = _tmp17 / (_tmp17 + std::pow(_tmp9, Scalar(2)));
|
||||
const Scalar _tmp23 =
|
||||
_tmp22 * (-_tmp18 * _tmp8 * (-_tmp15 - _tmp16) + _tmp21 * _tmp8 * (-_tmp19 + _tmp20));
|
||||
const Scalar _tmp24 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp25 = -_tmp0 + _tmp10;
|
||||
const Scalar _tmp26 =
|
||||
_tmp22 * (-_tmp18 * (_tmp12 * _tmp8 + _tmp3 * (_tmp1 - _tmp24 + _tmp25)) +
|
||||
_tmp21 * (_tmp3 * (_tmp11 - _tmp7) + _tmp8 * (-_tmp1 + _tmp24 + _tmp25)));
|
||||
// Intermediate terms (29)
|
||||
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 = 2 * _tmp1;
|
||||
const Scalar _tmp9 = std::pow(_tmp7, Scalar(2));
|
||||
const Scalar _tmp10 = _tmp5 / _tmp9;
|
||||
const Scalar _tmp11 = _tmp10 * _tmp8;
|
||||
const Scalar _tmp12 = 4 * _tmp1;
|
||||
const Scalar _tmp13 = 2 * _tmp4;
|
||||
const Scalar _tmp14 = Scalar(1.0) / (_tmp7);
|
||||
const Scalar _tmp15 =
|
||||
-_tmp11 * state(2, 0) + _tmp14 * (-_tmp12 * state(1, 0) + _tmp13 * state(2, 0));
|
||||
const Scalar _tmp16 = (Scalar(1) / Scalar(2)) * _tmp9 / (std::pow(_tmp5, Scalar(2)) + _tmp9);
|
||||
const Scalar _tmp17 = _tmp15 * _tmp16;
|
||||
const Scalar _tmp18 = _tmp13 * _tmp14;
|
||||
const Scalar _tmp19 = _tmp11 * state(3, 0) + _tmp18 * state(3, 0);
|
||||
const Scalar _tmp20 = _tmp16 * _tmp19;
|
||||
const Scalar _tmp21 = 4 * _tmp4;
|
||||
const Scalar _tmp22 = -_tmp10 * (-_tmp21 * state(3, 0) - _tmp8 * state(0, 0)) +
|
||||
_tmp14 * (-_tmp12 * state(3, 0) + _tmp13 * state(0, 0));
|
||||
const Scalar _tmp23 = _tmp16 * state(2, 0);
|
||||
const Scalar _tmp24 =
|
||||
_tmp16 * (-_tmp10 * (-_tmp21 * state(2, 0) + _tmp8 * state(1, 0)) + _tmp18 * state(1, 0));
|
||||
const Scalar _tmp25 =
|
||||
_tmp17 * state(0, 0) - _tmp20 * state(1, 0) + _tmp22 * _tmp23 - _tmp24 * state(3, 0);
|
||||
const Scalar _tmp26 = _tmp16 * _tmp22;
|
||||
const Scalar _tmp27 =
|
||||
_tmp22 * (-_tmp18 * _tmp3 * (_tmp15 + _tmp16) + _tmp21 * _tmp3 * (_tmp19 - _tmp20));
|
||||
_tmp17 * state(3, 0) - _tmp19 * _tmp23 + _tmp24 * state(0, 0) - _tmp26 * state(1, 0);
|
||||
const Scalar _tmp28 =
|
||||
-_tmp15 * _tmp23 - _tmp20 * state(3, 0) + _tmp24 * state(1, 0) + _tmp26 * state(0, 0);
|
||||
|
||||
// Output terms (3)
|
||||
if (meas_pred != nullptr) {
|
||||
Scalar& _meas_pred = (*meas_pred);
|
||||
|
||||
_meas_pred = std::atan2(_tmp9, _tmp14);
|
||||
_meas_pred = std::atan2(_tmp5, _tmp7);
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R + _tmp23 * (P(0, 1) * _tmp27 + P(1, 1) * _tmp23 + P(2, 1) * _tmp26) +
|
||||
_tmp26 * (P(0, 2) * _tmp27 + P(1, 2) * _tmp23 + P(2, 2) * _tmp26) +
|
||||
_tmp27 * (P(0, 0) * _tmp27 + P(1, 0) * _tmp23 + P(2, 0) * _tmp26);
|
||||
_innov_var = R + _tmp25 * (P(0, 0) * _tmp25 + P(1, 0) * _tmp27 + P(2, 0) * _tmp28) +
|
||||
_tmp27 * (P(0, 1) * _tmp25 + P(1, 1) * _tmp27 + P(2, 1) * _tmp28) +
|
||||
_tmp28 * (P(0, 2) * _tmp25 + P(1, 2) * _tmp27 + P(2, 2) * _tmp28);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
@@ -92,9 +97,9 @@ void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp27;
|
||||
_h(1, 0) = _tmp23;
|
||||
_h(2, 0) = _tmp26;
|
||||
_h(0, 0) = _tmp25;
|
||||
_h(1, 0) = _tmp27;
|
||||
_h(2, 0) = _tmp28;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+23
-26
@@ -29,38 +29,35 @@ void ComputeGravityXyzInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Hx = nullptr) {
|
||||
// Total ops: 51
|
||||
// Total ops: 53
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (16)
|
||||
const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp2 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp3 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp4 = _tmp0 + _tmp1 - _tmp2 - _tmp3;
|
||||
const Scalar _tmp5 = 2 * state(2, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp7 = 2 * state(1, 0);
|
||||
const Scalar _tmp8 = _tmp7 * state(0, 0);
|
||||
const Scalar _tmp9 = -_tmp6 - _tmp8;
|
||||
const Scalar _tmp10 = -_tmp0 - _tmp1 + _tmp2 + _tmp3;
|
||||
const Scalar _tmp11 = _tmp5 * state(0, 0);
|
||||
const Scalar _tmp12 = _tmp7 * state(3, 0);
|
||||
const Scalar _tmp13 = -_tmp11 + _tmp12;
|
||||
const Scalar _tmp14 = _tmp6 + _tmp8;
|
||||
const Scalar _tmp15 = _tmp11 - _tmp12;
|
||||
// Intermediate terms (13)
|
||||
const Scalar _tmp0 = 2 * state(0, 0);
|
||||
const Scalar _tmp1 = -_tmp0 * state(3, 0);
|
||||
const Scalar _tmp2 = 2 * state(2, 0);
|
||||
const Scalar _tmp3 = _tmp2 * state(1, 0);
|
||||
const Scalar _tmp4 = _tmp1 - _tmp3;
|
||||
const Scalar _tmp5 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp6 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp7 = std::pow(state(1, 0), Scalar(2)) - std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp8 = -_tmp5 + _tmp6 + _tmp7;
|
||||
const Scalar _tmp9 = _tmp1 + _tmp3;
|
||||
const Scalar _tmp10 = _tmp5 - _tmp6 + _tmp7;
|
||||
const Scalar _tmp11 = _tmp0 * state(1, 0) - _tmp2 * state(3, 0);
|
||||
const Scalar _tmp12 = _tmp2 * state(0, 0) + 2 * state(1, 0) * state(3, 0);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var(0, 0) = R + _tmp4 * (P(1, 1) * _tmp4 + P(2, 1) * _tmp9) +
|
||||
_tmp9 * (P(1, 2) * _tmp4 + P(2, 2) * _tmp9);
|
||||
_innov_var(1, 0) = R + _tmp10 * (P(0, 0) * _tmp10 + P(2, 0) * _tmp13) +
|
||||
_tmp13 * (P(0, 2) * _tmp10 + P(2, 2) * _tmp13);
|
||||
_innov_var(2, 0) = R + _tmp14 * (P(0, 0) * _tmp14 + P(1, 0) * _tmp15) +
|
||||
_tmp15 * (P(0, 1) * _tmp14 + P(1, 1) * _tmp15);
|
||||
_innov_var(0, 0) = R + _tmp4 * (P(0, 0) * _tmp4 + P(1, 0) * _tmp8) +
|
||||
_tmp8 * (P(0, 1) * _tmp4 + P(1, 1) * _tmp8);
|
||||
_innov_var(1, 0) = R + _tmp10 * (P(0, 0) * _tmp10 + P(1, 0) * _tmp9) +
|
||||
_tmp9 * (P(0, 1) * _tmp10 + P(1, 1) * _tmp9);
|
||||
_innov_var(2, 0) = R + _tmp11 * (P(0, 0) * _tmp11 + P(1, 0) * _tmp12) +
|
||||
_tmp12 * (P(0, 1) * _tmp11 + P(1, 1) * _tmp12);
|
||||
}
|
||||
|
||||
if (Hx != nullptr) {
|
||||
@@ -68,8 +65,8 @@ void ComputeGravityXyzInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_hx.setZero();
|
||||
|
||||
_hx(1, 0) = _tmp4;
|
||||
_hx(2, 0) = _tmp9;
|
||||
_hx(0, 0) = _tmp4;
|
||||
_hx(1, 0) = _tmp8;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+7
-7
@@ -34,16 +34,16 @@ void ComputeGravityYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (2)
|
||||
const Scalar _tmp0 = -std::pow(state(0, 0), Scalar(2)) + std::pow(state(1, 0), Scalar(2)) +
|
||||
std::pow(state(2, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = -2 * state(0, 0) * state(2, 0) + 2 * state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp0 = -2 * state(0, 0) * state(3, 0) + 2 * 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));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R + _tmp0 * (P(0, 0) * _tmp0 + P(2, 0) * _tmp1) +
|
||||
_tmp1 * (P(0, 2) * _tmp0 + P(2, 2) * _tmp1);
|
||||
_innov_var = R + _tmp0 * (P(0, 1) * _tmp1 + P(1, 1) * _tmp0) +
|
||||
_tmp1 * (P(0, 0) * _tmp1 + P(1, 0) * _tmp0);
|
||||
}
|
||||
|
||||
if (Hy != nullptr) {
|
||||
@@ -51,8 +51,8 @@ void ComputeGravityYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_hy.setZero();
|
||||
|
||||
_hy(0, 0) = _tmp0;
|
||||
_hy(2, 0) = _tmp1;
|
||||
_hy(0, 0) = _tmp1;
|
||||
_hy(1, 0) = _tmp0;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+2
-2
@@ -36,8 +36,8 @@ void ComputeGravityZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
// Intermediate terms (4)
|
||||
const Scalar _tmp0 = 2 * state(2, 0);
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 = _tmp0 * state(3, 0) + _tmp1 * state(0, 0);
|
||||
const Scalar _tmp3 = _tmp0 * state(0, 0) - _tmp1 * state(3, 0);
|
||||
const Scalar _tmp2 = -_tmp0 * state(3, 0) + _tmp1 * state(0, 0);
|
||||
const Scalar _tmp3 = _tmp0 * state(0, 0) + _tmp1 * state(3, 0);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
|
||||
+137
-97
@@ -35,114 +35,153 @@ 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, 23, 1>* const Hx = nullptr) {
|
||||
// Total ops: 314
|
||||
// Total ops: 461
|
||||
|
||||
// Unused inputs
|
||||
(void)epsilon;
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (47)
|
||||
const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = -2 * _tmp0;
|
||||
const Scalar _tmp2 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp3 = -2 * _tmp2;
|
||||
const Scalar _tmp4 = _tmp1 + _tmp3 + 1;
|
||||
const Scalar _tmp5 = 2 * state(0, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp7 = 2 * state(1, 0);
|
||||
const Scalar _tmp8 = _tmp7 * state(2, 0);
|
||||
const Scalar _tmp9 = _tmp6 + _tmp8;
|
||||
const Scalar _tmp10 = _tmp5 * state(2, 0);
|
||||
const Scalar _tmp11 = -_tmp10;
|
||||
const Scalar _tmp12 = _tmp7 * state(3, 0);
|
||||
const Scalar _tmp13 = _tmp11 + _tmp12;
|
||||
const Scalar _tmp14 = _tmp13 * state(18, 0) + _tmp9 * state(17, 0);
|
||||
const Scalar _tmp15 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp16 = 1 - 2 * _tmp15;
|
||||
const Scalar _tmp17 = _tmp1 + _tmp16;
|
||||
const Scalar _tmp18 = 2 * state(2, 0) * state(3, 0);
|
||||
const Scalar _tmp19 = _tmp7 * state(0, 0);
|
||||
const Scalar _tmp20 = _tmp18 + _tmp19;
|
||||
const Scalar _tmp21 = -_tmp6;
|
||||
const Scalar _tmp22 = _tmp21 + _tmp8;
|
||||
const Scalar _tmp23 = _tmp20 * state(18, 0) + _tmp22 * state(16, 0);
|
||||
const Scalar _tmp24 = _tmp16 + _tmp3;
|
||||
const Scalar _tmp25 = -_tmp19;
|
||||
const Scalar _tmp26 = _tmp18 + _tmp25;
|
||||
const Scalar _tmp27 = _tmp10 + _tmp12;
|
||||
const Scalar _tmp28 = _tmp26 * state(17, 0) + _tmp27 * state(16, 0);
|
||||
const Scalar _tmp29 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp30 = -_tmp29;
|
||||
const Scalar _tmp31 = _tmp2 + _tmp30;
|
||||
const Scalar _tmp32 = -_tmp0;
|
||||
const Scalar _tmp33 = _tmp15 + _tmp32;
|
||||
const Scalar _tmp34 = -_tmp18;
|
||||
const Scalar _tmp35 = -_tmp12;
|
||||
const Scalar _tmp36 = state(16, 0) * (_tmp11 + _tmp35) + state(17, 0) * (_tmp19 + _tmp34) +
|
||||
state(18, 0) * (_tmp31 + _tmp33);
|
||||
const Scalar _tmp37 = -_tmp15;
|
||||
const Scalar _tmp38 = _tmp23 + state(17, 0) * (_tmp2 + _tmp29 + _tmp32 + _tmp37);
|
||||
const Scalar _tmp39 = _tmp0 + _tmp37;
|
||||
const Scalar _tmp40 = -_tmp8;
|
||||
const Scalar _tmp41 = state(16, 0) * (_tmp31 + _tmp39) + state(17, 0) * (_tmp21 + _tmp40) +
|
||||
state(18, 0) * (_tmp10 + _tmp35);
|
||||
const Scalar _tmp42 = -_tmp2;
|
||||
const Scalar _tmp43 = _tmp29 + _tmp42;
|
||||
const Scalar _tmp44 = _tmp28 + state(18, 0) * (_tmp39 + _tmp43);
|
||||
const Scalar _tmp45 = _tmp14 + state(16, 0) * (_tmp33 + _tmp43);
|
||||
const Scalar _tmp46 = state(16, 0) * (_tmp40 + _tmp6) +
|
||||
state(17, 0) * (_tmp0 + _tmp15 + _tmp30 + _tmp42) +
|
||||
state(18, 0) * (_tmp25 + _tmp34);
|
||||
// Intermediate terms (68)
|
||||
const Scalar _tmp0 = -2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = 1 - 2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp2 = _tmp0 + _tmp1;
|
||||
const Scalar _tmp3 = 2 * state(3, 0);
|
||||
const Scalar _tmp4 = _tmp3 * state(0, 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 = -2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp13 = _tmp0 + _tmp12 + 1;
|
||||
const Scalar _tmp14 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp15 = _tmp9 * state(0, 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 = _tmp3 * state(18, 0);
|
||||
const Scalar _tmp22 = _tmp5 * state(17, 0);
|
||||
const Scalar _tmp23 = _tmp21 + _tmp22;
|
||||
const Scalar _tmp24 = (Scalar(1) / Scalar(2)) * state(2, 0);
|
||||
const Scalar _tmp25 = 2 * state(17, 0);
|
||||
const Scalar _tmp26 = _tmp25 * state(3, 0);
|
||||
const Scalar _tmp27 = _tmp5 * state(18, 0);
|
||||
const Scalar _tmp28 = _tmp26 - _tmp27;
|
||||
const Scalar _tmp29 = (Scalar(1) / Scalar(2)) * _tmp28;
|
||||
const Scalar _tmp30 = 4 * state(2, 0);
|
||||
const Scalar _tmp31 = _tmp9 * state(17, 0);
|
||||
const Scalar _tmp32 = 2 * state(0, 0);
|
||||
const Scalar _tmp33 = _tmp32 * state(18, 0);
|
||||
const Scalar _tmp34 = -_tmp30 * state(16, 0) + _tmp31 - _tmp33;
|
||||
const Scalar _tmp35 = (Scalar(1) / Scalar(2)) * state(1, 0);
|
||||
const Scalar _tmp36 = _tmp9 * state(18, 0);
|
||||
const Scalar _tmp37 = _tmp25 * state(0, 0);
|
||||
const Scalar _tmp38 = (Scalar(1) / Scalar(2)) * _tmp36 + (Scalar(1) / Scalar(2)) * _tmp37 -
|
||||
2 * state(16, 0) * state(3, 0);
|
||||
const Scalar _tmp39 =
|
||||
-_tmp23 * _tmp24 - _tmp29 * state(3, 0) + _tmp34 * _tmp35 + _tmp38 * state(0, 0);
|
||||
const Scalar _tmp40 = (Scalar(1) / Scalar(2)) * _tmp23;
|
||||
const Scalar _tmp41 = (Scalar(1) / Scalar(2)) * state(0, 0);
|
||||
const Scalar _tmp42 =
|
||||
-_tmp24 * _tmp28 + _tmp34 * _tmp41 - _tmp38 * state(1, 0) + _tmp40 * state(3, 0);
|
||||
const Scalar _tmp43 = (Scalar(1) / Scalar(2)) * state(3, 0);
|
||||
const Scalar _tmp44 =
|
||||
-_tmp29 * state(1, 0) - _tmp34 * _tmp43 + _tmp38 * state(2, 0) + _tmp40 * state(0, 0);
|
||||
const Scalar _tmp45 = _tmp3 * state(16, 0);
|
||||
const Scalar _tmp46 = _tmp36 - _tmp45;
|
||||
const Scalar _tmp47 = (Scalar(1) / Scalar(2)) * _tmp46;
|
||||
const Scalar _tmp48 = _tmp9 * state(16, 0);
|
||||
const Scalar _tmp49 = (Scalar(1) / Scalar(2)) * _tmp21 + (Scalar(1) / Scalar(2)) * _tmp48;
|
||||
const Scalar _tmp50 = _tmp5 * state(16, 0);
|
||||
const Scalar _tmp51 = 4 * state(17, 0);
|
||||
const Scalar _tmp52 = _tmp33 + _tmp50 - _tmp51 * state(1, 0);
|
||||
const Scalar _tmp53 = _tmp32 * state(16, 0);
|
||||
const Scalar _tmp54 = _tmp27 - _tmp51 * state(3, 0) - _tmp53;
|
||||
const Scalar _tmp55 =
|
||||
_tmp24 * _tmp54 + _tmp41 * _tmp52 - _tmp47 * state(1, 0) - _tmp49 * state(3, 0);
|
||||
const Scalar _tmp56 =
|
||||
-_tmp24 * _tmp52 + _tmp41 * _tmp54 - _tmp47 * state(3, 0) + _tmp49 * state(1, 0);
|
||||
const Scalar _tmp57 = -_tmp24 * _tmp46 - _tmp35 * _tmp54 + _tmp43 * _tmp52 + _tmp49 * state(0, 0);
|
||||
const Scalar _tmp58 = -_tmp31 + _tmp50;
|
||||
const Scalar _tmp59 = (Scalar(1) / Scalar(2)) * _tmp58;
|
||||
const Scalar _tmp60 = _tmp22 + _tmp48;
|
||||
const Scalar _tmp61 = (Scalar(1) / Scalar(2)) * _tmp60;
|
||||
const Scalar _tmp62 = _tmp26 - _tmp30 * state(18, 0) + _tmp53;
|
||||
const Scalar _tmp63 = -Scalar(1) / Scalar(2) * _tmp37 + (Scalar(1) / Scalar(2)) * _tmp45 -
|
||||
2 * state(1, 0) * state(18, 0);
|
||||
const Scalar _tmp64 =
|
||||
_tmp35 * _tmp62 - _tmp59 * state(3, 0) + _tmp61 * state(0, 0) - _tmp63 * state(2, 0);
|
||||
const Scalar _tmp65 = (Scalar(1) / Scalar(2)) * _tmp62;
|
||||
const Scalar _tmp66 =
|
||||
_tmp24 * _tmp60 - _tmp59 * state(1, 0) + _tmp63 * state(0, 0) - _tmp65 * state(3, 0);
|
||||
const Scalar _tmp67 =
|
||||
-_tmp24 * _tmp58 - _tmp61 * state(1, 0) + _tmp63 * state(3, 0) + _tmp65 * state(0, 0);
|
||||
|
||||
// Output terms (3)
|
||||
if (innov != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _innov = (*innov);
|
||||
|
||||
_innov(0, 0) = _tmp14 + _tmp4 * state(16, 0) - meas(0, 0) + state(19, 0);
|
||||
_innov(1, 0) = _tmp17 * state(17, 0) + _tmp23 - meas(1, 0) + state(20, 0);
|
||||
_innov(2, 0) = _tmp24 * state(18, 0) + _tmp28 - meas(2, 0) + state(21, 0);
|
||||
_innov(0, 0) = _tmp11 * state(18, 0) + _tmp2 * state(16, 0) + _tmp7 * state(17, 0) -
|
||||
meas(0, 0) + state(19, 0);
|
||||
_innov(1, 0) = _tmp13 * state(17, 0) + _tmp16 * state(18, 0) + _tmp17 * state(16, 0) -
|
||||
meas(1, 0) + state(20, 0);
|
||||
_innov(2, 0) = _tmp18 * state(18, 0) + _tmp19 * state(17, 0) + _tmp20 * state(16, 0) -
|
||||
meas(2, 0) + state(21, 0);
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var(0, 0) = P(1, 18) * _tmp36 + P(15, 18) * _tmp4 + P(16, 18) * _tmp9 +
|
||||
P(17, 18) * _tmp13 + P(18, 18) + P(2, 18) * _tmp38 + R +
|
||||
_tmp13 * (P(1, 17) * _tmp36 + P(15, 17) * _tmp4 + P(16, 17) * _tmp9 +
|
||||
P(17, 17) * _tmp13 + P(18, 17) + P(2, 17) * _tmp38) +
|
||||
_tmp36 * (P(1, 1) * _tmp36 + P(15, 1) * _tmp4 + P(16, 1) * _tmp9 +
|
||||
P(17, 1) * _tmp13 + P(18, 1) + P(2, 1) * _tmp38) +
|
||||
_tmp38 * (P(1, 2) * _tmp36 + P(15, 2) * _tmp4 + P(16, 2) * _tmp9 +
|
||||
P(17, 2) * _tmp13 + P(18, 2) + P(2, 2) * _tmp38) +
|
||||
_tmp4 * (P(1, 15) * _tmp36 + P(15, 15) * _tmp4 + P(16, 15) * _tmp9 +
|
||||
P(17, 15) * _tmp13 + P(18, 15) + P(2, 15) * _tmp38) +
|
||||
_tmp9 * (P(1, 16) * _tmp36 + P(15, 16) * _tmp4 + P(16, 16) * _tmp9 +
|
||||
P(17, 16) * _tmp13 + P(18, 16) + P(2, 16) * _tmp38);
|
||||
_innov_var(1, 0) = P(0, 19) * _tmp44 + P(15, 19) * _tmp22 + P(16, 19) * _tmp17 +
|
||||
P(17, 19) * _tmp20 + P(19, 19) + P(2, 19) * _tmp41 + R +
|
||||
_tmp17 * (P(0, 16) * _tmp44 + P(15, 16) * _tmp22 + P(16, 16) * _tmp17 +
|
||||
P(17, 16) * _tmp20 + P(19, 16) + P(2, 16) * _tmp41) +
|
||||
_tmp20 * (P(0, 17) * _tmp44 + P(15, 17) * _tmp22 + P(16, 17) * _tmp17 +
|
||||
P(17, 17) * _tmp20 + P(19, 17) + P(2, 17) * _tmp41) +
|
||||
_tmp22 * (P(0, 15) * _tmp44 + P(15, 15) * _tmp22 + P(16, 15) * _tmp17 +
|
||||
P(17, 15) * _tmp20 + P(19, 15) + P(2, 15) * _tmp41) +
|
||||
_tmp41 * (P(0, 2) * _tmp44 + P(15, 2) * _tmp22 + P(16, 2) * _tmp17 +
|
||||
P(17, 2) * _tmp20 + P(19, 2) + P(2, 2) * _tmp41) +
|
||||
_tmp44 * (P(0, 0) * _tmp44 + P(15, 0) * _tmp22 + P(16, 0) * _tmp17 +
|
||||
P(17, 0) * _tmp20 + P(19, 0) + P(2, 0) * _tmp41);
|
||||
_innov_var(2, 0) = P(0, 20) * _tmp46 + P(1, 20) * _tmp45 + P(15, 20) * _tmp27 +
|
||||
P(16, 20) * _tmp26 + P(17, 20) * _tmp24 + P(20, 20) + R +
|
||||
_tmp24 * (P(0, 17) * _tmp46 + P(1, 17) * _tmp45 + P(15, 17) * _tmp27 +
|
||||
P(16, 17) * _tmp26 + P(17, 17) * _tmp24 + P(20, 17)) +
|
||||
_tmp26 * (P(0, 16) * _tmp46 + P(1, 16) * _tmp45 + P(15, 16) * _tmp27 +
|
||||
P(16, 16) * _tmp26 + P(17, 16) * _tmp24 + P(20, 16)) +
|
||||
_tmp27 * (P(0, 15) * _tmp46 + P(1, 15) * _tmp45 + P(15, 15) * _tmp27 +
|
||||
P(16, 15) * _tmp26 + P(17, 15) * _tmp24 + P(20, 15)) +
|
||||
_tmp45 * (P(0, 1) * _tmp46 + P(1, 1) * _tmp45 + P(15, 1) * _tmp27 +
|
||||
P(16, 1) * _tmp26 + P(17, 1) * _tmp24 + P(20, 1)) +
|
||||
_tmp46 * (P(0, 0) * _tmp46 + P(1, 0) * _tmp45 + P(15, 0) * _tmp27 +
|
||||
P(16, 0) * _tmp26 + P(17, 0) * _tmp24 + P(20, 0));
|
||||
_innov_var(0, 0) =
|
||||
P(0, 18) * _tmp44 + P(1, 18) * _tmp42 + P(15, 18) * _tmp2 + P(16, 18) * _tmp7 +
|
||||
P(17, 18) * _tmp11 + P(18, 18) + P(2, 18) * _tmp39 + R +
|
||||
_tmp11 * (P(0, 17) * _tmp44 + P(1, 17) * _tmp42 + P(15, 17) * _tmp2 + P(16, 17) * _tmp7 +
|
||||
P(17, 17) * _tmp11 + P(18, 17) + P(2, 17) * _tmp39) +
|
||||
_tmp2 * (P(0, 15) * _tmp44 + P(1, 15) * _tmp42 + P(15, 15) * _tmp2 + P(16, 15) * _tmp7 +
|
||||
P(17, 15) * _tmp11 + P(18, 15) + P(2, 15) * _tmp39) +
|
||||
_tmp39 * (P(0, 2) * _tmp44 + P(1, 2) * _tmp42 + P(15, 2) * _tmp2 + P(16, 2) * _tmp7 +
|
||||
P(17, 2) * _tmp11 + P(18, 2) + P(2, 2) * _tmp39) +
|
||||
_tmp42 * (P(0, 1) * _tmp44 + P(1, 1) * _tmp42 + P(15, 1) * _tmp2 + P(16, 1) * _tmp7 +
|
||||
P(17, 1) * _tmp11 + P(18, 1) + P(2, 1) * _tmp39) +
|
||||
_tmp44 * (P(0, 0) * _tmp44 + P(1, 0) * _tmp42 + P(15, 0) * _tmp2 + P(16, 0) * _tmp7 +
|
||||
P(17, 0) * _tmp11 + P(18, 0) + P(2, 0) * _tmp39) +
|
||||
_tmp7 * (P(0, 16) * _tmp44 + P(1, 16) * _tmp42 + P(15, 16) * _tmp2 + P(16, 16) * _tmp7 +
|
||||
P(17, 16) * _tmp11 + P(18, 16) + P(2, 16) * _tmp39);
|
||||
_innov_var(1, 0) =
|
||||
P(0, 19) * _tmp55 + P(1, 19) * _tmp57 + P(15, 19) * _tmp17 + P(16, 19) * _tmp13 +
|
||||
P(17, 19) * _tmp16 + P(19, 19) + P(2, 19) * _tmp56 + R +
|
||||
_tmp13 * (P(0, 16) * _tmp55 + P(1, 16) * _tmp57 + P(15, 16) * _tmp17 + P(16, 16) * _tmp13 +
|
||||
P(17, 16) * _tmp16 + P(19, 16) + P(2, 16) * _tmp56) +
|
||||
_tmp16 * (P(0, 17) * _tmp55 + P(1, 17) * _tmp57 + P(15, 17) * _tmp17 + P(16, 17) * _tmp13 +
|
||||
P(17, 17) * _tmp16 + P(19, 17) + P(2, 17) * _tmp56) +
|
||||
_tmp17 * (P(0, 15) * _tmp55 + P(1, 15) * _tmp57 + P(15, 15) * _tmp17 + P(16, 15) * _tmp13 +
|
||||
P(17, 15) * _tmp16 + P(19, 15) + P(2, 15) * _tmp56) +
|
||||
_tmp55 * (P(0, 0) * _tmp55 + P(1, 0) * _tmp57 + P(15, 0) * _tmp17 + P(16, 0) * _tmp13 +
|
||||
P(17, 0) * _tmp16 + P(19, 0) + P(2, 0) * _tmp56) +
|
||||
_tmp56 * (P(0, 2) * _tmp55 + P(1, 2) * _tmp57 + P(15, 2) * _tmp17 + P(16, 2) * _tmp13 +
|
||||
P(17, 2) * _tmp16 + P(19, 2) + P(2, 2) * _tmp56) +
|
||||
_tmp57 * (P(0, 1) * _tmp55 + P(1, 1) * _tmp57 + P(15, 1) * _tmp17 + P(16, 1) * _tmp13 +
|
||||
P(17, 1) * _tmp16 + P(19, 1) + P(2, 1) * _tmp56);
|
||||
_innov_var(2, 0) =
|
||||
P(0, 20) * _tmp66 + P(1, 20) * _tmp67 + P(15, 20) * _tmp20 + P(16, 20) * _tmp19 +
|
||||
P(17, 20) * _tmp18 + P(2, 20) * _tmp64 + P(20, 20) + R +
|
||||
_tmp18 * (P(0, 17) * _tmp66 + P(1, 17) * _tmp67 + P(15, 17) * _tmp20 + P(16, 17) * _tmp19 +
|
||||
P(17, 17) * _tmp18 + P(2, 17) * _tmp64 + P(20, 17)) +
|
||||
_tmp19 * (P(0, 16) * _tmp66 + P(1, 16) * _tmp67 + P(15, 16) * _tmp20 + P(16, 16) * _tmp19 +
|
||||
P(17, 16) * _tmp18 + P(2, 16) * _tmp64 + P(20, 16)) +
|
||||
_tmp20 * (P(0, 15) * _tmp66 + P(1, 15) * _tmp67 + P(15, 15) * _tmp20 + P(16, 15) * _tmp19 +
|
||||
P(17, 15) * _tmp18 + P(2, 15) * _tmp64 + P(20, 15)) +
|
||||
_tmp64 * (P(0, 2) * _tmp66 + P(1, 2) * _tmp67 + P(15, 2) * _tmp20 + P(16, 2) * _tmp19 +
|
||||
P(17, 2) * _tmp18 + P(2, 2) * _tmp64 + P(20, 2)) +
|
||||
_tmp66 * (P(0, 0) * _tmp66 + P(1, 0) * _tmp67 + P(15, 0) * _tmp20 + P(16, 0) * _tmp19 +
|
||||
P(17, 0) * _tmp18 + P(2, 0) * _tmp64 + P(20, 0)) +
|
||||
_tmp67 * (P(0, 1) * _tmp66 + P(1, 1) * _tmp67 + P(15, 1) * _tmp20 + P(16, 1) * _tmp19 +
|
||||
P(17, 1) * _tmp18 + P(2, 1) * _tmp64 + P(20, 1));
|
||||
}
|
||||
|
||||
if (Hx != nullptr) {
|
||||
@@ -150,11 +189,12 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_hx.setZero();
|
||||
|
||||
_hx(1, 0) = _tmp36;
|
||||
_hx(2, 0) = _tmp38;
|
||||
_hx(15, 0) = _tmp4;
|
||||
_hx(16, 0) = _tmp9;
|
||||
_hx(17, 0) = _tmp13;
|
||||
_hx(0, 0) = _tmp44;
|
||||
_hx(1, 0) = _tmp42;
|
||||
_hx(2, 0) = _tmp39;
|
||||
_hx(15, 0) = _tmp2;
|
||||
_hx(16, 0) = _tmp7;
|
||||
_hx(17, 0) = _tmp11;
|
||||
_hx(18, 0) = 1;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
+45
-37
@@ -30,7 +30,7 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
// Total ops: 110
|
||||
// Total ops: 159
|
||||
|
||||
// Unused inputs
|
||||
(void)epsilon;
|
||||
@@ -38,43 +38,50 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (18)
|
||||
const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp2 = -2 * _tmp0 - 2 * _tmp1 + 1;
|
||||
const Scalar _tmp3 = 2 * state(2, 0);
|
||||
const Scalar _tmp4 = _tmp3 * state(3, 0);
|
||||
const Scalar _tmp5 = 2 * state(0, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(1, 0);
|
||||
const Scalar _tmp7 = _tmp4 + _tmp6;
|
||||
const Scalar _tmp8 = -_tmp5 * state(3, 0);
|
||||
const Scalar _tmp9 = _tmp3 * state(1, 0);
|
||||
const Scalar _tmp10 = _tmp8 + _tmp9;
|
||||
const Scalar _tmp11 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp12 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp13 = _tmp0 - _tmp1;
|
||||
const Scalar _tmp14 = _tmp3 * state(0, 0);
|
||||
const Scalar _tmp15 = 2 * state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp16 = state(16, 0) * (-_tmp11 + _tmp12 + _tmp13) +
|
||||
state(17, 0) * (_tmp8 - _tmp9) + state(18, 0) * (_tmp14 - _tmp15);
|
||||
const Scalar _tmp17 = state(16, 0) * (_tmp14 + _tmp15) + state(17, 0) * (_tmp4 - _tmp6) +
|
||||
state(18, 0) * (_tmp11 - _tmp12 + _tmp13);
|
||||
const Scalar _tmp0 = 2 * state(2, 0);
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 = _tmp0 * state(3, 0) + _tmp1 * state(0, 0);
|
||||
const Scalar _tmp3 = 2 * state(3, 0);
|
||||
const Scalar _tmp4 = _tmp0 * state(1, 0) - _tmp3 * state(0, 0);
|
||||
const Scalar _tmp5 =
|
||||
-2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
|
||||
const Scalar _tmp6 = _tmp1 * state(18, 0) - _tmp3 * state(16, 0);
|
||||
const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp6;
|
||||
const Scalar _tmp8 = (Scalar(1) / Scalar(2)) * _tmp1 * state(16, 0) +
|
||||
(Scalar(1) / Scalar(2)) * _tmp3 * state(18, 0);
|
||||
const Scalar _tmp9 = 4 * state(17, 0);
|
||||
const Scalar _tmp10 = 2 * state(0, 0);
|
||||
const Scalar _tmp11 = _tmp0 * state(16, 0) + _tmp10 * state(18, 0) - _tmp9 * state(1, 0);
|
||||
const Scalar _tmp12 = (Scalar(1) / Scalar(2)) * _tmp11;
|
||||
const Scalar _tmp13 = (Scalar(1) / Scalar(2)) * _tmp0 * state(18, 0) -
|
||||
Scalar(1) / Scalar(2) * _tmp10 * state(16, 0) -
|
||||
Scalar(1) / Scalar(2) * _tmp9 * state(3, 0);
|
||||
const Scalar _tmp14 =
|
||||
_tmp12 * state(0, 0) + _tmp13 * state(2, 0) - _tmp7 * state(1, 0) - _tmp8 * state(3, 0);
|
||||
const Scalar _tmp15 = (Scalar(1) / Scalar(2)) * state(2, 0);
|
||||
const Scalar _tmp16 =
|
||||
-_tmp11 * _tmp15 + _tmp13 * state(0, 0) - _tmp7 * state(3, 0) + _tmp8 * state(1, 0);
|
||||
const Scalar _tmp17 =
|
||||
_tmp12 * state(3, 0) - _tmp13 * state(1, 0) - _tmp15 * _tmp6 + _tmp8 * state(0, 0);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = P(0, 19) * _tmp17 + P(15, 19) * _tmp10 + P(16, 19) * _tmp2 + P(17, 19) * _tmp7 +
|
||||
P(19, 19) + P(2, 19) * _tmp16 + R +
|
||||
_tmp10 * (P(0, 15) * _tmp17 + P(15, 15) * _tmp10 + P(16, 15) * _tmp2 +
|
||||
P(17, 15) * _tmp7 + P(19, 15) + P(2, 15) * _tmp16) +
|
||||
_tmp16 * (P(0, 2) * _tmp17 + P(15, 2) * _tmp10 + P(16, 2) * _tmp2 +
|
||||
P(17, 2) * _tmp7 + P(19, 2) + P(2, 2) * _tmp16) +
|
||||
_tmp17 * (P(0, 0) * _tmp17 + P(15, 0) * _tmp10 + P(16, 0) * _tmp2 +
|
||||
P(17, 0) * _tmp7 + P(19, 0) + P(2, 0) * _tmp16) +
|
||||
_tmp2 * (P(0, 16) * _tmp17 + P(15, 16) * _tmp10 + P(16, 16) * _tmp2 +
|
||||
P(17, 16) * _tmp7 + P(19, 16) + P(2, 16) * _tmp16) +
|
||||
_tmp7 * (P(0, 17) * _tmp17 + P(15, 17) * _tmp10 + P(16, 17) * _tmp2 +
|
||||
P(17, 17) * _tmp7 + P(19, 17) + P(2, 17) * _tmp16);
|
||||
_innov_var = P(0, 19) * _tmp14 + P(1, 19) * _tmp17 + P(15, 19) * _tmp4 + P(16, 19) * _tmp5 +
|
||||
P(17, 19) * _tmp2 + P(19, 19) + P(2, 19) * _tmp16 + R +
|
||||
_tmp14 * (P(0, 0) * _tmp14 + P(1, 0) * _tmp17 + P(15, 0) * _tmp4 +
|
||||
P(16, 0) * _tmp5 + P(17, 0) * _tmp2 + P(19, 0) + P(2, 0) * _tmp16) +
|
||||
_tmp16 * (P(0, 2) * _tmp14 + P(1, 2) * _tmp17 + P(15, 2) * _tmp4 +
|
||||
P(16, 2) * _tmp5 + P(17, 2) * _tmp2 + P(19, 2) + P(2, 2) * _tmp16) +
|
||||
_tmp17 * (P(0, 1) * _tmp14 + P(1, 1) * _tmp17 + P(15, 1) * _tmp4 +
|
||||
P(16, 1) * _tmp5 + P(17, 1) * _tmp2 + P(19, 1) + P(2, 1) * _tmp16) +
|
||||
_tmp2 * (P(0, 17) * _tmp14 + P(1, 17) * _tmp17 + P(15, 17) * _tmp4 +
|
||||
P(16, 17) * _tmp5 + P(17, 17) * _tmp2 + P(19, 17) + P(2, 17) * _tmp16) +
|
||||
_tmp4 * (P(0, 15) * _tmp14 + P(1, 15) * _tmp17 + P(15, 15) * _tmp4 +
|
||||
P(16, 15) * _tmp5 + P(17, 15) * _tmp2 + P(19, 15) + P(2, 15) * _tmp16) +
|
||||
_tmp5 * (P(0, 16) * _tmp14 + P(1, 16) * _tmp17 + P(15, 16) * _tmp4 +
|
||||
P(16, 16) * _tmp5 + P(17, 16) * _tmp2 + P(19, 16) + P(2, 16) * _tmp16);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
@@ -82,11 +89,12 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp17;
|
||||
_h(0, 0) = _tmp14;
|
||||
_h(1, 0) = _tmp17;
|
||||
_h(2, 0) = _tmp16;
|
||||
_h(15, 0) = _tmp10;
|
||||
_h(16, 0) = _tmp2;
|
||||
_h(17, 0) = _tmp7;
|
||||
_h(15, 0) = _tmp4;
|
||||
_h(16, 0) = _tmp5;
|
||||
_h(17, 0) = _tmp2;
|
||||
_h(19, 0) = 1;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
+47
-39
@@ -30,51 +30,58 @@ void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
// Total ops: 110
|
||||
// Total ops: 161
|
||||
|
||||
// Unused inputs
|
||||
(void)epsilon;
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (18)
|
||||
const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp2 = -2 * _tmp0 - 2 * _tmp1 + 1;
|
||||
const Scalar _tmp3 = 2 * state(2, 0);
|
||||
const Scalar _tmp4 = _tmp3 * state(3, 0);
|
||||
const Scalar _tmp5 = 2 * state(0, 0);
|
||||
const Scalar _tmp6 = -_tmp5 * state(1, 0);
|
||||
const Scalar _tmp7 = _tmp4 + _tmp6;
|
||||
const Scalar _tmp8 = _tmp3 * state(0, 0);
|
||||
const Scalar _tmp9 = 2 * state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp10 = _tmp8 + _tmp9;
|
||||
const Scalar _tmp11 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp12 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp13 = -_tmp0 + _tmp1;
|
||||
const Scalar _tmp14 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp15 = _tmp3 * state(1, 0);
|
||||
const Scalar _tmp16 = state(16, 0) * (-_tmp11 + _tmp12 + _tmp13) +
|
||||
state(17, 0) * (_tmp14 + _tmp15) + state(18, 0) * (-_tmp8 + _tmp9);
|
||||
const Scalar _tmp17 = state(16, 0) * (_tmp14 - _tmp15) +
|
||||
state(17, 0) * (_tmp11 - _tmp12 + _tmp13) + state(18, 0) * (-_tmp4 + _tmp6);
|
||||
// Intermediate terms (15)
|
||||
const Scalar _tmp0 = 2 * state(16, 0);
|
||||
const Scalar _tmp1 = 2 * state(17, 0);
|
||||
const Scalar _tmp2 =
|
||||
(Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) - Scalar(1) / Scalar(2) * _tmp1 * state(1, 0);
|
||||
const Scalar _tmp3 = 2 * state(1, 0);
|
||||
const Scalar _tmp4 = (Scalar(1) / Scalar(2)) * _tmp1 * state(2, 0) +
|
||||
(Scalar(1) / Scalar(2)) * _tmp3 * state(16, 0);
|
||||
const Scalar _tmp5 = 4 * state(18, 0);
|
||||
const Scalar _tmp6 = (Scalar(1) / Scalar(2)) * _tmp0 * state(0, 0) +
|
||||
(Scalar(1) / Scalar(2)) * _tmp1 * state(3, 0) -
|
||||
Scalar(1) / Scalar(2) * _tmp5 * state(2, 0);
|
||||
const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp0 * state(3, 0) -
|
||||
Scalar(1) / Scalar(2) * _tmp1 * state(0, 0) -
|
||||
Scalar(1) / Scalar(2) * _tmp5 * state(1, 0);
|
||||
const Scalar _tmp8 =
|
||||
-_tmp2 * state(3, 0) + _tmp4 * state(0, 0) + _tmp6 * state(1, 0) - _tmp7 * state(2, 0);
|
||||
const Scalar _tmp9 =
|
||||
-_tmp2 * state(1, 0) + _tmp4 * state(2, 0) - _tmp6 * state(3, 0) + _tmp7 * state(0, 0);
|
||||
const Scalar _tmp10 =
|
||||
-_tmp2 * state(2, 0) - _tmp4 * state(1, 0) + _tmp6 * state(0, 0) + _tmp7 * state(3, 0);
|
||||
const Scalar _tmp11 =
|
||||
-2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1;
|
||||
const Scalar _tmp12 = 2 * state(2, 0);
|
||||
const Scalar _tmp13 = _tmp12 * state(3, 0) - _tmp3 * state(0, 0);
|
||||
const Scalar _tmp14 = _tmp12 * state(0, 0) + _tmp3 * state(3, 0);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = P(0, 20) * _tmp17 + P(1, 20) * _tmp16 + P(15, 20) * _tmp10 + P(16, 20) * _tmp7 +
|
||||
P(17, 20) * _tmp2 + P(20, 20) + R +
|
||||
_tmp10 * (P(0, 15) * _tmp17 + P(1, 15) * _tmp16 + P(15, 15) * _tmp10 +
|
||||
P(16, 15) * _tmp7 + P(17, 15) * _tmp2 + P(20, 15)) +
|
||||
_tmp16 * (P(0, 1) * _tmp17 + P(1, 1) * _tmp16 + P(15, 1) * _tmp10 +
|
||||
P(16, 1) * _tmp7 + P(17, 1) * _tmp2 + P(20, 1)) +
|
||||
_tmp17 * (P(0, 0) * _tmp17 + P(1, 0) * _tmp16 + P(15, 0) * _tmp10 +
|
||||
P(16, 0) * _tmp7 + P(17, 0) * _tmp2 + P(20, 0)) +
|
||||
_tmp2 * (P(0, 17) * _tmp17 + P(1, 17) * _tmp16 + P(15, 17) * _tmp10 +
|
||||
P(16, 17) * _tmp7 + P(17, 17) * _tmp2 + P(20, 17)) +
|
||||
_tmp7 * (P(0, 16) * _tmp17 + P(1, 16) * _tmp16 + P(15, 16) * _tmp10 +
|
||||
P(16, 16) * _tmp7 + P(17, 16) * _tmp2 + P(20, 16));
|
||||
_innov_var = P(0, 20) * _tmp9 + P(1, 20) * _tmp10 + P(15, 20) * _tmp14 + P(16, 20) * _tmp13 +
|
||||
P(17, 20) * _tmp11 + P(2, 20) * _tmp8 + P(20, 20) + R +
|
||||
_tmp10 * (P(0, 1) * _tmp9 + P(1, 1) * _tmp10 + P(15, 1) * _tmp14 +
|
||||
P(16, 1) * _tmp13 + P(17, 1) * _tmp11 + P(2, 1) * _tmp8 + P(20, 1)) +
|
||||
_tmp11 * (P(0, 17) * _tmp9 + P(1, 17) * _tmp10 + P(15, 17) * _tmp14 +
|
||||
P(16, 17) * _tmp13 + P(17, 17) * _tmp11 + P(2, 17) * _tmp8 + P(20, 17)) +
|
||||
_tmp13 * (P(0, 16) * _tmp9 + P(1, 16) * _tmp10 + P(15, 16) * _tmp14 +
|
||||
P(16, 16) * _tmp13 + P(17, 16) * _tmp11 + P(2, 16) * _tmp8 + P(20, 16)) +
|
||||
_tmp14 * (P(0, 15) * _tmp9 + P(1, 15) * _tmp10 + P(15, 15) * _tmp14 +
|
||||
P(16, 15) * _tmp13 + P(17, 15) * _tmp11 + P(2, 15) * _tmp8 + P(20, 15)) +
|
||||
_tmp8 * (P(0, 2) * _tmp9 + P(1, 2) * _tmp10 + P(15, 2) * _tmp14 +
|
||||
P(16, 2) * _tmp13 + P(17, 2) * _tmp11 + P(2, 2) * _tmp8 + P(20, 2)) +
|
||||
_tmp9 * (P(0, 0) * _tmp9 + P(1, 0) * _tmp10 + P(15, 0) * _tmp14 +
|
||||
P(16, 0) * _tmp13 + P(17, 0) * _tmp11 + P(2, 0) * _tmp8 + P(20, 0));
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
@@ -82,11 +89,12 @@ void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp17;
|
||||
_h(1, 0) = _tmp16;
|
||||
_h(15, 0) = _tmp10;
|
||||
_h(16, 0) = _tmp7;
|
||||
_h(17, 0) = _tmp2;
|
||||
_h(0, 0) = _tmp9;
|
||||
_h(1, 0) = _tmp10;
|
||||
_h(2, 0) = _tmp8;
|
||||
_h(15, 0) = _tmp14;
|
||||
_h(16, 0) = _tmp13;
|
||||
_h(17, 0) = _tmp11;
|
||||
_h(20, 0) = 1;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
@@ -30,63 +30,66 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar innov_var,
|
||||
const Scalar epsilon, matrix::Matrix<Scalar, 23, 1>* const H = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const K = nullptr) {
|
||||
// Total ops: 469
|
||||
// Total ops: 497
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (46)
|
||||
const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp2 = 1 - 2 * _tmp1;
|
||||
const Scalar _tmp3 = -2 * _tmp0 + _tmp2;
|
||||
const Scalar _tmp4 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp5 = 2 * state(0, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp7 = 2 * state(2, 0);
|
||||
const Scalar _tmp8 = _tmp7 * state(1, 0);
|
||||
const Scalar _tmp9 = _tmp6 + _tmp8;
|
||||
const Scalar _tmp10 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp11 = _tmp7 * state(0, 0);
|
||||
const Scalar _tmp12 = -_tmp11;
|
||||
const Scalar _tmp13 = 2 * state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp14 = _tmp12 + _tmp13;
|
||||
const Scalar _tmp15 = _tmp10 * _tmp9 + _tmp14 * state(6, 0) + _tmp3 * _tmp4;
|
||||
const Scalar _tmp16 =
|
||||
_tmp15 + epsilon * (2 * math::min<Scalar>(0, (((_tmp15) > 0) - ((_tmp15) < 0))) + 1);
|
||||
const Scalar _tmp17 = Scalar(1.0) / (_tmp16);
|
||||
const Scalar _tmp18 = _tmp7 * state(3, 0);
|
||||
const Scalar _tmp19 = _tmp5 * state(1, 0);
|
||||
const Scalar _tmp20 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp21 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp22 = -_tmp21;
|
||||
const Scalar _tmp23 = _tmp20 + _tmp22;
|
||||
const Scalar _tmp24 = _tmp17 * (_tmp10 * (_tmp18 - _tmp19) + _tmp4 * (_tmp11 + _tmp13) +
|
||||
state(6, 0) * (-_tmp0 + _tmp1 + _tmp23));
|
||||
const Scalar _tmp25 = -_tmp13;
|
||||
const Scalar _tmp26 = -_tmp20;
|
||||
const Scalar _tmp27 = _tmp0 - _tmp1;
|
||||
const Scalar _tmp28 = _tmp2 - 2 * _tmp21;
|
||||
const Scalar _tmp29 = -_tmp6;
|
||||
const Scalar _tmp30 = _tmp29 + _tmp8;
|
||||
const Scalar _tmp31 = _tmp18 + _tmp19;
|
||||
const Scalar _tmp32 = _tmp30 * _tmp4 + _tmp31 * state(6, 0);
|
||||
const Scalar _tmp33 = (_tmp10 * _tmp28 + _tmp32) / std::pow(_tmp16, Scalar(2));
|
||||
const Scalar _tmp34 = _tmp33 * (_tmp10 * (-_tmp18 + _tmp19) + _tmp4 * (_tmp12 + _tmp25) +
|
||||
state(6, 0) * (_tmp21 + _tmp26 + _tmp27));
|
||||
const Scalar _tmp35 =
|
||||
_tmp17 * (_tmp10 * (_tmp29 - _tmp8) + _tmp4 * (_tmp0 + _tmp1 + _tmp22 + _tmp26) +
|
||||
state(6, 0) * (_tmp11 + _tmp25)) -
|
||||
_tmp33 * (_tmp10 * (_tmp23 + _tmp27) + _tmp32);
|
||||
const Scalar _tmp36 = _tmp3 * _tmp33;
|
||||
const Scalar _tmp37 = _tmp17 * _tmp30;
|
||||
const Scalar _tmp38 = -_tmp36 + _tmp37;
|
||||
const Scalar _tmp39 = _tmp33 * _tmp9;
|
||||
const Scalar _tmp40 = _tmp17 * _tmp28;
|
||||
const Scalar _tmp41 = -_tmp39 + _tmp40;
|
||||
const Scalar _tmp42 = -_tmp14 * _tmp33 + _tmp17 * _tmp31;
|
||||
const Scalar _tmp43 = _tmp36 - _tmp37;
|
||||
const Scalar _tmp44 = _tmp39 - _tmp40;
|
||||
const Scalar _tmp45 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
|
||||
// Intermediate terms (45)
|
||||
const Scalar _tmp0 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp3 = 2 * state(6, 0);
|
||||
const Scalar _tmp4 = _tmp3 * state(0, 0);
|
||||
const Scalar _tmp5 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp6 = _tmp5 - 2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp7 = 2 * state(0, 0);
|
||||
const Scalar _tmp8 = _tmp7 * state(3, 0);
|
||||
const Scalar _tmp9 = 2 * state(2, 0);
|
||||
const Scalar _tmp10 = _tmp9 * state(1, 0);
|
||||
const Scalar _tmp11 = _tmp10 + _tmp8;
|
||||
const Scalar _tmp12 = _tmp1 * state(3, 0) - _tmp9 * state(0, 0);
|
||||
const Scalar _tmp13 = _tmp0 * _tmp11 + _tmp12 * state(6, 0) + _tmp2 * _tmp6;
|
||||
const Scalar _tmp14 =
|
||||
_tmp13 + epsilon * (2 * math::min<Scalar>(0, (((_tmp13) > 0) - ((_tmp13) < 0))) + 1);
|
||||
const Scalar _tmp15 = _tmp5 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp16 = _tmp10 - _tmp8;
|
||||
const Scalar _tmp17 = _tmp7 * state(1, 0) + _tmp9 * state(3, 0);
|
||||
const Scalar _tmp18 =
|
||||
(_tmp0 * _tmp15 + _tmp16 * _tmp2 + _tmp17 * state(6, 0)) / std::pow(_tmp14, Scalar(2));
|
||||
const Scalar _tmp19 = _tmp3 * state(3, 0);
|
||||
const Scalar _tmp20 = Scalar(1.0) / (_tmp14);
|
||||
const Scalar _tmp21 = -_tmp18 * (_tmp0 * _tmp1 - 4 * _tmp2 * state(2, 0) - _tmp4) +
|
||||
_tmp20 * (_tmp1 * _tmp2 + _tmp19);
|
||||
const Scalar _tmp22 = (Scalar(1) / Scalar(2)) * state(3, 0);
|
||||
const Scalar _tmp23 =
|
||||
-Scalar(1) / Scalar(2) * _tmp18 * (_tmp0 * _tmp9 + _tmp19) +
|
||||
(Scalar(1) / Scalar(2)) * _tmp20 * (-4 * _tmp0 * state(1, 0) + _tmp2 * _tmp9 + _tmp4);
|
||||
const Scalar _tmp24 = 2 * state(3, 0);
|
||||
const Scalar _tmp25 = _tmp3 * state(2, 0);
|
||||
const Scalar _tmp26 = _tmp3 * state(1, 0);
|
||||
const Scalar _tmp27 = -_tmp18 * (_tmp0 * _tmp24 - _tmp25) + _tmp20 * (-_tmp2 * _tmp24 + _tmp26);
|
||||
const Scalar _tmp28 = (Scalar(1) / Scalar(2)) * _tmp27;
|
||||
const Scalar _tmp29 = 4 * state(3, 0);
|
||||
const Scalar _tmp30 =
|
||||
-Scalar(1) / Scalar(2) * _tmp18 * (_tmp0 * _tmp7 - _tmp2 * _tmp29 + _tmp26) +
|
||||
(Scalar(1) / Scalar(2)) * _tmp20 * (-_tmp0 * _tmp29 - _tmp2 * _tmp7 + _tmp25);
|
||||
const Scalar _tmp31 =
|
||||
-_tmp21 * _tmp22 + _tmp23 * state(0, 0) - _tmp28 * state(1, 0) + _tmp30 * state(2, 0);
|
||||
const Scalar _tmp32 = (Scalar(1) / Scalar(2)) * _tmp21;
|
||||
const Scalar _tmp33 =
|
||||
_tmp23 * state(3, 0) - _tmp28 * state(2, 0) - _tmp30 * state(1, 0) + _tmp32 * state(0, 0);
|
||||
const Scalar _tmp34 =
|
||||
-_tmp22 * _tmp27 - _tmp23 * state(2, 0) + _tmp30 * state(0, 0) + _tmp32 * state(1, 0);
|
||||
const Scalar _tmp35 = _tmp18 * _tmp6;
|
||||
const Scalar _tmp36 = _tmp16 * _tmp20;
|
||||
const Scalar _tmp37 = -_tmp35 + _tmp36;
|
||||
const Scalar _tmp38 = _tmp11 * _tmp18;
|
||||
const Scalar _tmp39 = _tmp15 * _tmp20;
|
||||
const Scalar _tmp40 = -_tmp38 + _tmp39;
|
||||
const Scalar _tmp41 = -_tmp12 * _tmp18 + _tmp17 * _tmp20;
|
||||
const Scalar _tmp42 = _tmp35 - _tmp36;
|
||||
const Scalar _tmp43 = _tmp38 - _tmp39;
|
||||
const Scalar _tmp44 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
|
||||
|
||||
// Output terms (2)
|
||||
if (H != nullptr) {
|
||||
@@ -94,88 +97,88 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp24;
|
||||
_h(1, 0) = -_tmp34;
|
||||
_h(2, 0) = _tmp35;
|
||||
_h(3, 0) = _tmp38;
|
||||
_h(4, 0) = _tmp41;
|
||||
_h(5, 0) = _tmp42;
|
||||
_h(21, 0) = _tmp43;
|
||||
_h(22, 0) = _tmp44;
|
||||
_h(0, 0) = _tmp31;
|
||||
_h(1, 0) = _tmp33;
|
||||
_h(2, 0) = _tmp34;
|
||||
_h(3, 0) = _tmp37;
|
||||
_h(4, 0) = _tmp40;
|
||||
_h(5, 0) = _tmp41;
|
||||
_h(21, 0) = _tmp42;
|
||||
_h(22, 0) = _tmp43;
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _k = (*K);
|
||||
|
||||
_k(0, 0) =
|
||||
_tmp45 * (P(0, 0) * _tmp24 - P(0, 1) * _tmp34 + P(0, 2) * _tmp35 + P(0, 21) * _tmp43 +
|
||||
P(0, 22) * _tmp44 + P(0, 3) * _tmp38 + P(0, 4) * _tmp41 + P(0, 5) * _tmp42);
|
||||
_tmp44 * (P(0, 0) * _tmp31 + P(0, 1) * _tmp33 + P(0, 2) * _tmp34 + P(0, 21) * _tmp42 +
|
||||
P(0, 22) * _tmp43 + P(0, 3) * _tmp37 + P(0, 4) * _tmp40 + P(0, 5) * _tmp41);
|
||||
_k(1, 0) =
|
||||
_tmp45 * (P(1, 0) * _tmp24 - P(1, 1) * _tmp34 + P(1, 2) * _tmp35 + P(1, 21) * _tmp43 +
|
||||
P(1, 22) * _tmp44 + P(1, 3) * _tmp38 + P(1, 4) * _tmp41 + P(1, 5) * _tmp42);
|
||||
_tmp44 * (P(1, 0) * _tmp31 + P(1, 1) * _tmp33 + P(1, 2) * _tmp34 + P(1, 21) * _tmp42 +
|
||||
P(1, 22) * _tmp43 + P(1, 3) * _tmp37 + P(1, 4) * _tmp40 + P(1, 5) * _tmp41);
|
||||
_k(2, 0) =
|
||||
_tmp45 * (P(2, 0) * _tmp24 - P(2, 1) * _tmp34 + P(2, 2) * _tmp35 + P(2, 21) * _tmp43 +
|
||||
P(2, 22) * _tmp44 + P(2, 3) * _tmp38 + P(2, 4) * _tmp41 + P(2, 5) * _tmp42);
|
||||
_tmp44 * (P(2, 0) * _tmp31 + P(2, 1) * _tmp33 + P(2, 2) * _tmp34 + P(2, 21) * _tmp42 +
|
||||
P(2, 22) * _tmp43 + P(2, 3) * _tmp37 + P(2, 4) * _tmp40 + P(2, 5) * _tmp41);
|
||||
_k(3, 0) =
|
||||
_tmp45 * (P(3, 0) * _tmp24 - P(3, 1) * _tmp34 + P(3, 2) * _tmp35 + P(3, 21) * _tmp43 +
|
||||
P(3, 22) * _tmp44 + P(3, 3) * _tmp38 + P(3, 4) * _tmp41 + P(3, 5) * _tmp42);
|
||||
_tmp44 * (P(3, 0) * _tmp31 + P(3, 1) * _tmp33 + P(3, 2) * _tmp34 + P(3, 21) * _tmp42 +
|
||||
P(3, 22) * _tmp43 + P(3, 3) * _tmp37 + P(3, 4) * _tmp40 + P(3, 5) * _tmp41);
|
||||
_k(4, 0) =
|
||||
_tmp45 * (P(4, 0) * _tmp24 - P(4, 1) * _tmp34 + P(4, 2) * _tmp35 + P(4, 21) * _tmp43 +
|
||||
P(4, 22) * _tmp44 + P(4, 3) * _tmp38 + P(4, 4) * _tmp41 + P(4, 5) * _tmp42);
|
||||
_tmp44 * (P(4, 0) * _tmp31 + P(4, 1) * _tmp33 + P(4, 2) * _tmp34 + P(4, 21) * _tmp42 +
|
||||
P(4, 22) * _tmp43 + P(4, 3) * _tmp37 + P(4, 4) * _tmp40 + P(4, 5) * _tmp41);
|
||||
_k(5, 0) =
|
||||
_tmp45 * (P(5, 0) * _tmp24 - P(5, 1) * _tmp34 + P(5, 2) * _tmp35 + P(5, 21) * _tmp43 +
|
||||
P(5, 22) * _tmp44 + P(5, 3) * _tmp38 + P(5, 4) * _tmp41 + P(5, 5) * _tmp42);
|
||||
_tmp44 * (P(5, 0) * _tmp31 + P(5, 1) * _tmp33 + P(5, 2) * _tmp34 + P(5, 21) * _tmp42 +
|
||||
P(5, 22) * _tmp43 + P(5, 3) * _tmp37 + P(5, 4) * _tmp40 + P(5, 5) * _tmp41);
|
||||
_k(6, 0) =
|
||||
_tmp45 * (P(6, 0) * _tmp24 - P(6, 1) * _tmp34 + P(6, 2) * _tmp35 + P(6, 21) * _tmp43 +
|
||||
P(6, 22) * _tmp44 + P(6, 3) * _tmp38 + P(6, 4) * _tmp41 + P(6, 5) * _tmp42);
|
||||
_tmp44 * (P(6, 0) * _tmp31 + P(6, 1) * _tmp33 + P(6, 2) * _tmp34 + P(6, 21) * _tmp42 +
|
||||
P(6, 22) * _tmp43 + P(6, 3) * _tmp37 + P(6, 4) * _tmp40 + P(6, 5) * _tmp41);
|
||||
_k(7, 0) =
|
||||
_tmp45 * (P(7, 0) * _tmp24 - P(7, 1) * _tmp34 + P(7, 2) * _tmp35 + P(7, 21) * _tmp43 +
|
||||
P(7, 22) * _tmp44 + P(7, 3) * _tmp38 + P(7, 4) * _tmp41 + P(7, 5) * _tmp42);
|
||||
_tmp44 * (P(7, 0) * _tmp31 + P(7, 1) * _tmp33 + P(7, 2) * _tmp34 + P(7, 21) * _tmp42 +
|
||||
P(7, 22) * _tmp43 + P(7, 3) * _tmp37 + P(7, 4) * _tmp40 + P(7, 5) * _tmp41);
|
||||
_k(8, 0) =
|
||||
_tmp45 * (P(8, 0) * _tmp24 - P(8, 1) * _tmp34 + P(8, 2) * _tmp35 + P(8, 21) * _tmp43 +
|
||||
P(8, 22) * _tmp44 + P(8, 3) * _tmp38 + P(8, 4) * _tmp41 + P(8, 5) * _tmp42);
|
||||
_tmp44 * (P(8, 0) * _tmp31 + P(8, 1) * _tmp33 + P(8, 2) * _tmp34 + P(8, 21) * _tmp42 +
|
||||
P(8, 22) * _tmp43 + P(8, 3) * _tmp37 + P(8, 4) * _tmp40 + P(8, 5) * _tmp41);
|
||||
_k(9, 0) =
|
||||
_tmp45 * (P(9, 0) * _tmp24 - P(9, 1) * _tmp34 + P(9, 2) * _tmp35 + P(9, 21) * _tmp43 +
|
||||
P(9, 22) * _tmp44 + P(9, 3) * _tmp38 + P(9, 4) * _tmp41 + P(9, 5) * _tmp42);
|
||||
_tmp44 * (P(9, 0) * _tmp31 + P(9, 1) * _tmp33 + P(9, 2) * _tmp34 + P(9, 21) * _tmp42 +
|
||||
P(9, 22) * _tmp43 + P(9, 3) * _tmp37 + P(9, 4) * _tmp40 + P(9, 5) * _tmp41);
|
||||
_k(10, 0) =
|
||||
_tmp45 * (P(10, 0) * _tmp24 - P(10, 1) * _tmp34 + P(10, 2) * _tmp35 + P(10, 21) * _tmp43 +
|
||||
P(10, 22) * _tmp44 + P(10, 3) * _tmp38 + P(10, 4) * _tmp41 + P(10, 5) * _tmp42);
|
||||
_tmp44 * (P(10, 0) * _tmp31 + P(10, 1) * _tmp33 + P(10, 2) * _tmp34 + P(10, 21) * _tmp42 +
|
||||
P(10, 22) * _tmp43 + P(10, 3) * _tmp37 + P(10, 4) * _tmp40 + P(10, 5) * _tmp41);
|
||||
_k(11, 0) =
|
||||
_tmp45 * (P(11, 0) * _tmp24 - P(11, 1) * _tmp34 + P(11, 2) * _tmp35 + P(11, 21) * _tmp43 +
|
||||
P(11, 22) * _tmp44 + P(11, 3) * _tmp38 + P(11, 4) * _tmp41 + P(11, 5) * _tmp42);
|
||||
_tmp44 * (P(11, 0) * _tmp31 + P(11, 1) * _tmp33 + P(11, 2) * _tmp34 + P(11, 21) * _tmp42 +
|
||||
P(11, 22) * _tmp43 + P(11, 3) * _tmp37 + P(11, 4) * _tmp40 + P(11, 5) * _tmp41);
|
||||
_k(12, 0) =
|
||||
_tmp45 * (P(12, 0) * _tmp24 - P(12, 1) * _tmp34 + P(12, 2) * _tmp35 + P(12, 21) * _tmp43 +
|
||||
P(12, 22) * _tmp44 + P(12, 3) * _tmp38 + P(12, 4) * _tmp41 + P(12, 5) * _tmp42);
|
||||
_tmp44 * (P(12, 0) * _tmp31 + P(12, 1) * _tmp33 + P(12, 2) * _tmp34 + P(12, 21) * _tmp42 +
|
||||
P(12, 22) * _tmp43 + P(12, 3) * _tmp37 + P(12, 4) * _tmp40 + P(12, 5) * _tmp41);
|
||||
_k(13, 0) =
|
||||
_tmp45 * (P(13, 0) * _tmp24 - P(13, 1) * _tmp34 + P(13, 2) * _tmp35 + P(13, 21) * _tmp43 +
|
||||
P(13, 22) * _tmp44 + P(13, 3) * _tmp38 + P(13, 4) * _tmp41 + P(13, 5) * _tmp42);
|
||||
_tmp44 * (P(13, 0) * _tmp31 + P(13, 1) * _tmp33 + P(13, 2) * _tmp34 + P(13, 21) * _tmp42 +
|
||||
P(13, 22) * _tmp43 + P(13, 3) * _tmp37 + P(13, 4) * _tmp40 + P(13, 5) * _tmp41);
|
||||
_k(14, 0) =
|
||||
_tmp45 * (P(14, 0) * _tmp24 - P(14, 1) * _tmp34 + P(14, 2) * _tmp35 + P(14, 21) * _tmp43 +
|
||||
P(14, 22) * _tmp44 + P(14, 3) * _tmp38 + P(14, 4) * _tmp41 + P(14, 5) * _tmp42);
|
||||
_tmp44 * (P(14, 0) * _tmp31 + P(14, 1) * _tmp33 + P(14, 2) * _tmp34 + P(14, 21) * _tmp42 +
|
||||
P(14, 22) * _tmp43 + P(14, 3) * _tmp37 + P(14, 4) * _tmp40 + P(14, 5) * _tmp41);
|
||||
_k(15, 0) =
|
||||
_tmp45 * (P(15, 0) * _tmp24 - P(15, 1) * _tmp34 + P(15, 2) * _tmp35 + P(15, 21) * _tmp43 +
|
||||
P(15, 22) * _tmp44 + P(15, 3) * _tmp38 + P(15, 4) * _tmp41 + P(15, 5) * _tmp42);
|
||||
_tmp44 * (P(15, 0) * _tmp31 + P(15, 1) * _tmp33 + P(15, 2) * _tmp34 + P(15, 21) * _tmp42 +
|
||||
P(15, 22) * _tmp43 + P(15, 3) * _tmp37 + P(15, 4) * _tmp40 + P(15, 5) * _tmp41);
|
||||
_k(16, 0) =
|
||||
_tmp45 * (P(16, 0) * _tmp24 - P(16, 1) * _tmp34 + P(16, 2) * _tmp35 + P(16, 21) * _tmp43 +
|
||||
P(16, 22) * _tmp44 + P(16, 3) * _tmp38 + P(16, 4) * _tmp41 + P(16, 5) * _tmp42);
|
||||
_tmp44 * (P(16, 0) * _tmp31 + P(16, 1) * _tmp33 + P(16, 2) * _tmp34 + P(16, 21) * _tmp42 +
|
||||
P(16, 22) * _tmp43 + P(16, 3) * _tmp37 + P(16, 4) * _tmp40 + P(16, 5) * _tmp41);
|
||||
_k(17, 0) =
|
||||
_tmp45 * (P(17, 0) * _tmp24 - P(17, 1) * _tmp34 + P(17, 2) * _tmp35 + P(17, 21) * _tmp43 +
|
||||
P(17, 22) * _tmp44 + P(17, 3) * _tmp38 + P(17, 4) * _tmp41 + P(17, 5) * _tmp42);
|
||||
_tmp44 * (P(17, 0) * _tmp31 + P(17, 1) * _tmp33 + P(17, 2) * _tmp34 + P(17, 21) * _tmp42 +
|
||||
P(17, 22) * _tmp43 + P(17, 3) * _tmp37 + P(17, 4) * _tmp40 + P(17, 5) * _tmp41);
|
||||
_k(18, 0) =
|
||||
_tmp45 * (P(18, 0) * _tmp24 - P(18, 1) * _tmp34 + P(18, 2) * _tmp35 + P(18, 21) * _tmp43 +
|
||||
P(18, 22) * _tmp44 + P(18, 3) * _tmp38 + P(18, 4) * _tmp41 + P(18, 5) * _tmp42);
|
||||
_tmp44 * (P(18, 0) * _tmp31 + P(18, 1) * _tmp33 + P(18, 2) * _tmp34 + P(18, 21) * _tmp42 +
|
||||
P(18, 22) * _tmp43 + P(18, 3) * _tmp37 + P(18, 4) * _tmp40 + P(18, 5) * _tmp41);
|
||||
_k(19, 0) =
|
||||
_tmp45 * (P(19, 0) * _tmp24 - P(19, 1) * _tmp34 + P(19, 2) * _tmp35 + P(19, 21) * _tmp43 +
|
||||
P(19, 22) * _tmp44 + P(19, 3) * _tmp38 + P(19, 4) * _tmp41 + P(19, 5) * _tmp42);
|
||||
_tmp44 * (P(19, 0) * _tmp31 + P(19, 1) * _tmp33 + P(19, 2) * _tmp34 + P(19, 21) * _tmp42 +
|
||||
P(19, 22) * _tmp43 + P(19, 3) * _tmp37 + P(19, 4) * _tmp40 + P(19, 5) * _tmp41);
|
||||
_k(20, 0) =
|
||||
_tmp45 * (P(20, 0) * _tmp24 - P(20, 1) * _tmp34 + P(20, 2) * _tmp35 + P(20, 21) * _tmp43 +
|
||||
P(20, 22) * _tmp44 + P(20, 3) * _tmp38 + P(20, 4) * _tmp41 + P(20, 5) * _tmp42);
|
||||
_tmp44 * (P(20, 0) * _tmp31 + P(20, 1) * _tmp33 + P(20, 2) * _tmp34 + P(20, 21) * _tmp42 +
|
||||
P(20, 22) * _tmp43 + P(20, 3) * _tmp37 + P(20, 4) * _tmp40 + P(20, 5) * _tmp41);
|
||||
_k(21, 0) =
|
||||
_tmp45 * (P(21, 0) * _tmp24 - P(21, 1) * _tmp34 + P(21, 2) * _tmp35 + P(21, 21) * _tmp43 +
|
||||
P(21, 22) * _tmp44 + P(21, 3) * _tmp38 + P(21, 4) * _tmp41 + P(21, 5) * _tmp42);
|
||||
_tmp44 * (P(21, 0) * _tmp31 + P(21, 1) * _tmp33 + P(21, 2) * _tmp34 + P(21, 21) * _tmp42 +
|
||||
P(21, 22) * _tmp43 + P(21, 3) * _tmp37 + P(21, 4) * _tmp40 + P(21, 5) * _tmp41);
|
||||
_k(22, 0) =
|
||||
_tmp45 * (P(22, 0) * _tmp24 - P(22, 1) * _tmp34 + P(22, 2) * _tmp35 + P(22, 21) * _tmp43 +
|
||||
P(22, 22) * _tmp44 + P(22, 3) * _tmp38 + P(22, 4) * _tmp41 + P(22, 5) * _tmp42);
|
||||
_tmp44 * (P(22, 0) * _tmp31 + P(22, 1) * _tmp33 + P(22, 2) * _tmp34 + P(22, 21) * _tmp42 +
|
||||
P(22, 22) * _tmp43 + P(22, 3) * _tmp37 + P(22, 4) * _tmp40 + P(22, 5) * _tmp41);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+71
-70
@@ -30,69 +30,70 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov = nullptr,
|
||||
Scalar* const innov_var = nullptr) {
|
||||
// Total ops: 235
|
||||
// Total ops: 265
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (46)
|
||||
const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp2 = 1 - 2 * _tmp1;
|
||||
const Scalar _tmp3 = -2 * _tmp0 + _tmp2;
|
||||
const Scalar _tmp4 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp5 = 2 * state(0, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp7 = 2 * state(1, 0);
|
||||
const Scalar _tmp8 = _tmp7 * state(2, 0);
|
||||
const Scalar _tmp9 = _tmp6 + _tmp8;
|
||||
const Scalar _tmp10 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp11 = _tmp5 * state(2, 0);
|
||||
const Scalar _tmp12 = -_tmp11;
|
||||
const Scalar _tmp13 = _tmp7 * state(3, 0);
|
||||
const Scalar _tmp14 = _tmp12 + _tmp13;
|
||||
const Scalar _tmp15 = _tmp10 * _tmp9 + _tmp14 * state(6, 0) + _tmp3 * _tmp4;
|
||||
const Scalar _tmp16 =
|
||||
_tmp15 + epsilon * (2 * math::min<Scalar>(0, (((_tmp15) > 0) - ((_tmp15) < 0))) + 1);
|
||||
const Scalar _tmp17 = Scalar(1.0) / (_tmp16);
|
||||
const Scalar _tmp18 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp19 = -2 * _tmp18 + _tmp2;
|
||||
const Scalar _tmp20 = -_tmp6;
|
||||
const Scalar _tmp21 = _tmp20 + _tmp8;
|
||||
const Scalar _tmp22 = 2 * state(2, 0) * state(3, 0);
|
||||
const Scalar _tmp23 = _tmp5 * state(1, 0);
|
||||
const Scalar _tmp24 = _tmp22 + _tmp23;
|
||||
const Scalar _tmp25 = _tmp21 * _tmp4 + _tmp24 * state(6, 0);
|
||||
const Scalar _tmp26 = _tmp10 * _tmp19 + _tmp25;
|
||||
const Scalar _tmp27 = _tmp26 / std::pow(_tmp16, Scalar(2));
|
||||
const Scalar _tmp28 = -_tmp14 * _tmp27 + _tmp17 * _tmp24;
|
||||
const Scalar _tmp29 = _tmp27 * _tmp3;
|
||||
const Scalar _tmp30 = _tmp17 * _tmp21;
|
||||
const Scalar _tmp31 = -_tmp29 + _tmp30;
|
||||
const Scalar _tmp32 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp33 = -_tmp32;
|
||||
const Scalar _tmp34 = -_tmp18;
|
||||
const Scalar _tmp35 = -_tmp13;
|
||||
const Scalar _tmp36 = _tmp0 - _tmp1;
|
||||
const Scalar _tmp37 = _tmp32 + _tmp34;
|
||||
// Intermediate terms (42)
|
||||
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(0, 0);
|
||||
const Scalar _tmp4 = _tmp3 * state(3, 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 * state(2, 0);
|
||||
const Scalar _tmp10 = _tmp5 * state(3, 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 = _tmp5 * state(0, 0) + _tmp9 * state(3, 0);
|
||||
const Scalar _tmp17 = _tmp14 * _tmp8 + _tmp15 * _tmp2 + _tmp16 * state(6, 0);
|
||||
const Scalar _tmp18 = _tmp17 / std::pow(_tmp12, Scalar(2));
|
||||
const Scalar _tmp19 = _tmp18 * _tmp7;
|
||||
const Scalar _tmp20 = _tmp13 * _tmp14;
|
||||
const Scalar _tmp21 = _tmp19 - _tmp20;
|
||||
const Scalar _tmp22 = -_tmp10 * _tmp18 + _tmp13 * _tmp16;
|
||||
const Scalar _tmp23 = _tmp1 * _tmp18;
|
||||
const Scalar _tmp24 = _tmp13 * _tmp15;
|
||||
const Scalar _tmp25 = -_tmp23 + _tmp24;
|
||||
const Scalar _tmp26 = 2 * state(6, 0);
|
||||
const Scalar _tmp27 = _tmp26 * state(0, 0);
|
||||
const Scalar _tmp28 = _tmp26 * state(3, 0);
|
||||
const Scalar _tmp29 =
|
||||
(Scalar(1) / Scalar(2)) * _tmp13 * (_tmp2 * _tmp5 + _tmp28) -
|
||||
Scalar(1) / Scalar(2) * _tmp18 * (-4 * _tmp2 * state(2, 0) - _tmp27 + _tmp5 * _tmp8);
|
||||
const Scalar _tmp30 =
|
||||
(Scalar(1) / Scalar(2)) * _tmp13 * (_tmp2 * _tmp9 + _tmp27 - 4 * _tmp8 * state(1, 0)) -
|
||||
Scalar(1) / Scalar(2) * _tmp18 * (_tmp28 + _tmp8 * _tmp9);
|
||||
const Scalar _tmp31 = 2 * state(3, 0);
|
||||
const Scalar _tmp32 = _tmp26 * state(2, 0);
|
||||
const Scalar _tmp33 = _tmp26 * state(1, 0);
|
||||
const Scalar _tmp34 = (Scalar(1) / Scalar(2)) * _tmp13 * (-_tmp2 * _tmp31 + _tmp33) -
|
||||
Scalar(1) / Scalar(2) * _tmp18 * (_tmp31 * _tmp8 - _tmp32);
|
||||
const Scalar _tmp35 = 4 * state(3, 0);
|
||||
const Scalar _tmp36 =
|
||||
(Scalar(1) / Scalar(2)) * _tmp13 * (-_tmp2 * _tmp3 + _tmp32 - _tmp35 * _tmp8) -
|
||||
Scalar(1) / Scalar(2) * _tmp18 * (-_tmp2 * _tmp35 + _tmp3 * _tmp8 + _tmp33);
|
||||
const Scalar _tmp37 =
|
||||
_tmp29 * state(1, 0) - _tmp30 * state(2, 0) - _tmp34 * state(3, 0) + _tmp36 * state(0, 0);
|
||||
const Scalar _tmp38 =
|
||||
_tmp17 * (_tmp10 * (_tmp20 - _tmp8) + _tmp4 * (_tmp0 + _tmp1 + _tmp33 + _tmp34) +
|
||||
state(6, 0) * (_tmp11 + _tmp35)) -
|
||||
_tmp27 * (_tmp10 * (_tmp36 + _tmp37) + _tmp25);
|
||||
const Scalar _tmp39 = _tmp29 - _tmp30;
|
||||
const Scalar _tmp40 = _tmp27 * _tmp9;
|
||||
const Scalar _tmp41 = _tmp17 * _tmp19;
|
||||
const Scalar _tmp42 = -_tmp40 + _tmp41;
|
||||
const Scalar _tmp43 = _tmp27 * (_tmp10 * (-_tmp22 + _tmp23) + _tmp4 * (_tmp12 + _tmp35) +
|
||||
state(6, 0) * (_tmp18 + _tmp33 + _tmp36));
|
||||
const Scalar _tmp44 = _tmp40 - _tmp41;
|
||||
const Scalar _tmp45 = _tmp17 * (_tmp10 * (_tmp22 - _tmp23) + _tmp4 * (_tmp11 + _tmp13) +
|
||||
state(6, 0) * (-_tmp0 + _tmp1 + _tmp37));
|
||||
-_tmp29 * state(3, 0) + _tmp30 * state(0, 0) - _tmp34 * state(1, 0) + _tmp36 * state(2, 0);
|
||||
const Scalar _tmp39 =
|
||||
_tmp29 * state(0, 0) + _tmp30 * state(3, 0) - _tmp34 * state(2, 0) - _tmp36 * state(1, 0);
|
||||
const Scalar _tmp40 = _tmp23 - _tmp24;
|
||||
const Scalar _tmp41 = -_tmp19 + _tmp20;
|
||||
|
||||
// Output terms (2)
|
||||
if (innov != nullptr) {
|
||||
Scalar& _innov = (*innov);
|
||||
|
||||
_innov = _tmp17 * _tmp26;
|
||||
_innov = _tmp13 * _tmp17;
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
@@ -100,22 +101,22 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_innov_var =
|
||||
R +
|
||||
_tmp28 * (P(0, 5) * _tmp45 - P(1, 5) * _tmp43 + P(2, 5) * _tmp38 + P(21, 5) * _tmp39 +
|
||||
P(22, 5) * _tmp44 + P(3, 5) * _tmp31 + P(4, 5) * _tmp42 + P(5, 5) * _tmp28) +
|
||||
_tmp31 * (P(0, 3) * _tmp45 - P(1, 3) * _tmp43 + P(2, 3) * _tmp38 + P(21, 3) * _tmp39 +
|
||||
P(22, 3) * _tmp44 + P(3, 3) * _tmp31 + P(4, 3) * _tmp42 + P(5, 3) * _tmp28) +
|
||||
_tmp38 * (P(0, 2) * _tmp45 - P(1, 2) * _tmp43 + P(2, 2) * _tmp38 + P(21, 2) * _tmp39 +
|
||||
P(22, 2) * _tmp44 + P(3, 2) * _tmp31 + P(4, 2) * _tmp42 + P(5, 2) * _tmp28) +
|
||||
_tmp39 * (P(0, 21) * _tmp45 - P(1, 21) * _tmp43 + P(2, 21) * _tmp38 + P(21, 21) * _tmp39 +
|
||||
P(22, 21) * _tmp44 + P(3, 21) * _tmp31 + P(4, 21) * _tmp42 + P(5, 21) * _tmp28) +
|
||||
_tmp42 * (P(0, 4) * _tmp45 - P(1, 4) * _tmp43 + P(2, 4) * _tmp38 + P(21, 4) * _tmp39 +
|
||||
P(22, 4) * _tmp44 + P(3, 4) * _tmp31 + P(4, 4) * _tmp42 + P(5, 4) * _tmp28) -
|
||||
_tmp43 * (P(0, 1) * _tmp45 - P(1, 1) * _tmp43 + P(2, 1) * _tmp38 + P(21, 1) * _tmp39 +
|
||||
P(22, 1) * _tmp44 + P(3, 1) * _tmp31 + P(4, 1) * _tmp42 + P(5, 1) * _tmp28) +
|
||||
_tmp44 * (P(0, 22) * _tmp45 - P(1, 22) * _tmp43 + P(2, 22) * _tmp38 + P(21, 22) * _tmp39 +
|
||||
P(22, 22) * _tmp44 + P(3, 22) * _tmp31 + P(4, 22) * _tmp42 + P(5, 22) * _tmp28) +
|
||||
_tmp45 * (P(0, 0) * _tmp45 - P(1, 0) * _tmp43 + P(2, 0) * _tmp38 + P(21, 0) * _tmp39 +
|
||||
P(22, 0) * _tmp44 + P(3, 0) * _tmp31 + P(4, 0) * _tmp42 + P(5, 0) * _tmp28);
|
||||
_tmp21 * (P(0, 22) * _tmp38 + P(1, 22) * _tmp39 + P(2, 22) * _tmp37 + P(21, 22) * _tmp40 +
|
||||
P(22, 22) * _tmp21 + P(3, 22) * _tmp25 + P(4, 22) * _tmp41 + P(5, 22) * _tmp22) +
|
||||
_tmp22 * (P(0, 5) * _tmp38 + P(1, 5) * _tmp39 + P(2, 5) * _tmp37 + P(21, 5) * _tmp40 +
|
||||
P(22, 5) * _tmp21 + P(3, 5) * _tmp25 + P(4, 5) * _tmp41 + P(5, 5) * _tmp22) +
|
||||
_tmp25 * (P(0, 3) * _tmp38 + P(1, 3) * _tmp39 + P(2, 3) * _tmp37 + P(21, 3) * _tmp40 +
|
||||
P(22, 3) * _tmp21 + P(3, 3) * _tmp25 + P(4, 3) * _tmp41 + P(5, 3) * _tmp22) +
|
||||
_tmp37 * (P(0, 2) * _tmp38 + P(1, 2) * _tmp39 + P(2, 2) * _tmp37 + P(21, 2) * _tmp40 +
|
||||
P(22, 2) * _tmp21 + P(3, 2) * _tmp25 + P(4, 2) * _tmp41 + P(5, 2) * _tmp22) +
|
||||
_tmp38 * (P(0, 0) * _tmp38 + P(1, 0) * _tmp39 + P(2, 0) * _tmp37 + P(21, 0) * _tmp40 +
|
||||
P(22, 0) * _tmp21 + P(3, 0) * _tmp25 + P(4, 0) * _tmp41 + P(5, 0) * _tmp22) +
|
||||
_tmp39 * (P(0, 1) * _tmp38 + P(1, 1) * _tmp39 + P(2, 1) * _tmp37 + P(21, 1) * _tmp40 +
|
||||
P(22, 1) * _tmp21 + P(3, 1) * _tmp25 + P(4, 1) * _tmp41 + P(5, 1) * _tmp22) +
|
||||
_tmp40 * (P(0, 21) * _tmp38 + P(1, 21) * _tmp39 + P(2, 21) * _tmp37 + P(21, 21) * _tmp40 +
|
||||
P(22, 21) * _tmp21 + P(3, 21) * _tmp25 + P(4, 21) * _tmp41 + P(5, 21) * _tmp22) +
|
||||
_tmp41 * (P(0, 4) * _tmp38 + P(1, 4) * _tmp39 + P(2, 4) * _tmp37 + P(21, 4) * _tmp40 +
|
||||
P(22, 4) * _tmp21 + P(3, 4) * _tmp25 + P(4, 4) * _tmp41 + P(5, 4) * _tmp22);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
@@ -29,25 +29,20 @@ void ComputeYawInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
// Total ops: 36
|
||||
// Total ops: 1
|
||||
|
||||
// Unused inputs
|
||||
(void)state;
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (5)
|
||||
const Scalar _tmp0 = 2 * state(2, 0);
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 = -_tmp0 * state(0, 0) + _tmp1 * state(3, 0);
|
||||
const Scalar _tmp3 = _tmp0 * state(3, 0) + _tmp1 * state(0, 0);
|
||||
const Scalar _tmp4 = 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 (0)
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R + _tmp2 * (P(0, 0) * _tmp2 + P(1, 0) * _tmp3 + P(2, 0) * _tmp4) +
|
||||
_tmp3 * (P(0, 1) * _tmp2 + P(1, 1) * _tmp3 + P(2, 1) * _tmp4) +
|
||||
_tmp4 * (P(0, 2) * _tmp2 + P(1, 2) * _tmp3 + P(2, 2) * _tmp4);
|
||||
_innov_var = P(2, 2) + R;
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
@@ -55,9 +50,7 @@ void ComputeYawInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp2;
|
||||
_h(1, 0) = _tmp3;
|
||||
_h(2, 0) = _tmp4;
|
||||
_h(2, 0) = 1;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1658,7 +1658,7 @@ void EKF2::PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu_sa
|
||||
_ekf.getPositionVariance().copyTo(odom.position_variance);
|
||||
|
||||
// orientation covariance
|
||||
_ekf.getQuaternionVariance().copyTo(odom.orientation_variance);
|
||||
_ekf.getRotVarBody().copyTo(odom.orientation_variance);
|
||||
|
||||
odom.reset_counter = _ekf.get_quat_reset_count()
|
||||
+ _ekf.get_velNE_reset_count() + _ekf.get_velD_reset_count()
|
||||
|
||||
@@ -258,10 +258,12 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithAutopilotGyroData)
|
||||
|
||||
// THEN: the flow due to the yaw rotation and the offsets is canceled
|
||||
// and the velocity estimate stays 0
|
||||
// FIXME: the estimate isn't perfect 0 mainly because the mag simulated measurement isn't rotating
|
||||
// and this creates an incorrect Z gyro bias which is used to compensate for the apparent flow
|
||||
const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());
|
||||
EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.01f)
|
||||
EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.02f)
|
||||
<< "estimated vel = " << estimated_horz_velocity(0);
|
||||
EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.01f)
|
||||
EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.02f)
|
||||
<< "estimated vel = " << estimated_horz_velocity(1);
|
||||
}
|
||||
|
||||
|
||||
@@ -78,7 +78,7 @@ public:
|
||||
|
||||
void quaternionVarianceBigEnoughAfterOrientationInitialization(float quat_variance_limit = 0.00001f)
|
||||
{
|
||||
const matrix::Vector3f quat_variance = _ekf->getQuaternionVariance();
|
||||
const matrix::Vector3f quat_variance = _ekf->getRotVarBody();
|
||||
EXPECT_TRUE(quat_variance(0) > quat_variance_limit) << "quat_variance(3): " << quat_variance(0);
|
||||
EXPECT_TRUE(quat_variance(1) > quat_variance_limit) << "quat_variance(1): " << quat_variance(1);
|
||||
EXPECT_TRUE(quat_variance(2) > quat_variance_limit) << "quat_variance(2): " << quat_variance(2);
|
||||
|
||||
@@ -42,9 +42,8 @@ using namespace matrix;
|
||||
Vector3f getRotVarNed(const Quatf &q, const SquareMatrixState &P)
|
||||
{
|
||||
constexpr auto S = State::quat_nominal;
|
||||
matrix::SquareMatrix3f rot_cov_body = P.slice<S.dof, S.dof>(S.idx, S.idx);
|
||||
auto R_to_earth = Dcmf(q);
|
||||
return matrix::SquareMatrix<float, State::quat_nominal.dof>(R_to_earth * rot_cov_body * R_to_earth.T()).diag();
|
||||
matrix::SquareMatrix3f rot_cov_ned = P.slice<S.dof, S.dof>(S.idx, S.idx);
|
||||
return rot_cov_ned.diag();
|
||||
}
|
||||
|
||||
TEST(YawFusionGenerated, positiveVarianceAllOrientations)
|
||||
|
||||
Reference in New Issue
Block a user