forked from Archive/PX4-Autopilot
flow: add distance exponent factor
This commit is contained in:
bresch
parent
48785db562
commit
0dc71e1867
|
|
@ -1,7 +1,7 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
|
||||
|
||||
float32[25] states # Internal filter states
|
||||
float32[26] states # Internal filter states
|
||||
uint8 n_states # Number of states effectively used
|
||||
|
||||
float32[24] covariances # Diagonal Elements of Covariance Matrix
|
||||
float32[25] covariances # Diagonal Elements of Covariance Matrix
|
||||
|
|
|
|||
|
|
@ -100,6 +100,7 @@ void Ekf::initialiseCovariance()
|
|||
resetWindCov();
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
P(State::flow_scale.idx, State::flow_scale.idx) = sq(0.1f);
|
||||
P(State::flow_exp.idx, State::flow_exp.idx) = sq(0.1f);
|
||||
}
|
||||
|
||||
void Ekf::predictCovariance(const imuSample &imu_delayed)
|
||||
|
|
@ -210,6 +211,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
|
|||
if (_control_status.flags.opt_flow) {
|
||||
const float flow_scale_nsd = 0.1f;
|
||||
P(State::flow_scale.idx, State::flow_scale.idx) += sq(flow_scale_nsd) * dt;
|
||||
P(State::flow_exp.idx, State::flow_exp.idx) += sq(flow_scale_nsd) * dt;
|
||||
}
|
||||
|
||||
// covariance matrix is symmetrical, so copy upper half to lower half
|
||||
|
|
|
|||
|
|
@ -72,6 +72,7 @@ void Ekf::reset()
|
|||
_state.wind_vel.setZero();
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
_state.flow_scale = 1.f;
|
||||
_state.flow_exp = 1.f;
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_range_sensor.setPitchOffset(_params.rng_sens_pitch);
|
||||
|
|
|
|||
|
|
@ -775,6 +775,7 @@ void Ekf::fuse(const VectorState &K, float innovation)
|
|||
_state.wind_vel -= K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) * innovation;
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
_state.flow_scale -= (K.slice<State::flow_scale.dof, 1>(State::flow_scale.idx, 0) * innovation)(0, 0);
|
||||
_state.flow_exp -= (K.slice<State::flow_exp.dof, 1>(State::flow_exp.idx, 0) * innovation)(0, 0);
|
||||
}
|
||||
|
||||
void Ekf::uncorrelateQuatFromOtherStates()
|
||||
|
|
|
|||
|
|
@ -368,6 +368,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
|
|||
|
||||
flowSample optflow_sample_new{flow};
|
||||
optflow_sample_new.time_us = time_us;
|
||||
optflow_sample_new.flow_xy_rad *= 0.82f;
|
||||
|
||||
_flow_buffer->push(optflow_sample_new);
|
||||
|
||||
|
|
|
|||
|
|
@ -61,16 +61,17 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
|
|||
// Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
|
||||
const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt;
|
||||
|
||||
const float range_scaled = std::pow(range, _state.flow_exp);
|
||||
// compute the velocities in body and local frames from corrected optical flow measurement for logging only
|
||||
_flow_vel_body(0) = -opt_flow_rate(1) * range;
|
||||
_flow_vel_body(1) = opt_flow_rate(0) * range;
|
||||
_flow_vel_body(0) = -opt_flow_rate(1) * range_scaled / _state.flow_scale;
|
||||
_flow_vel_body(1) = opt_flow_rate(0) * range_scaled / _state.flow_scale;
|
||||
_flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f));
|
||||
|
||||
aid_src.observation[0] = opt_flow_rate(0); // flow around the X axis
|
||||
aid_src.observation[1] = opt_flow_rate(1); // flow around the Y axis
|
||||
|
||||
aid_src.innovation[0] = _state.flow_scale * (vel_body(1) / range) - aid_src.observation[0];
|
||||
aid_src.innovation[1] = _state.flow_scale * (-vel_body(0) / range) - aid_src.observation[1];
|
||||
aid_src.innovation[0] = _state.flow_scale * (vel_body(1) / range_scaled) - aid_src.observation[0];
|
||||
aid_src.innovation[1] = _state.flow_scale * (-vel_body(0) / range_scaled) - aid_src.observation[1];
|
||||
|
||||
// calculate the optical flow observation variance
|
||||
const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed);
|
||||
|
|
@ -134,7 +135,7 @@ void Ekf::fuseOptFlow()
|
|||
// recalculate the innovation using the updated state
|
||||
const Vector2f vel_body = predictFlowVelBody();
|
||||
range = predictFlowRange();
|
||||
_aid_src_optical_flow.innovation[1] = _state.flow_scale * (-vel_body(0) / range) - _aid_src_optical_flow.observation[1];
|
||||
_aid_src_optical_flow.innovation[1] = _state.flow_scale * (-vel_body(0) / std::pow(range, _state.flow_exp)) - _aid_src_optical_flow.observation[1];
|
||||
|
||||
if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) {
|
||||
// we need to reinitialise the covariance matrix and abort this fusion step
|
||||
|
|
|
|||
|
|
@ -63,7 +63,8 @@ State = Values(
|
|||
mag_I = sf.V3(),
|
||||
mag_B = sf.V3(),
|
||||
wind_vel = sf.V2(),
|
||||
flow_scale = sf.V1()
|
||||
flow_scale = sf.V1(),
|
||||
flow_exp = sf.V1()
|
||||
)
|
||||
|
||||
if args.disable_mag:
|
||||
|
|
@ -129,7 +130,8 @@ def predict_covariance(
|
|||
mag_I = sf.V3.symbolic("mag_I"),
|
||||
mag_B = sf.V3.symbolic("mag_B"),
|
||||
wind_vel = sf.V2.symbolic("wind_vel"),
|
||||
flow_scale = sf.V1.symbolic("flow_scale")
|
||||
flow_scale = sf.V1.symbolic("flow_scale"),
|
||||
flow_exp = sf.V1.symbolic("flow_exp")
|
||||
)
|
||||
|
||||
if args.disable_mag:
|
||||
|
|
@ -473,10 +475,11 @@ def predict_opt_flow(state, distance, epsilon):
|
|||
# Divide by range to get predicted angular LOS rates relative to X and Y
|
||||
# axes. Note these are rates in a non-rotating sensor frame
|
||||
flow_pred = sf.V2()
|
||||
flow_pred[0] = rel_vel_sensor[1] / distance
|
||||
flow_pred[1] = -rel_vel_sensor[0] / distance
|
||||
dist_scaled = distance**state["flow_exp"][0]
|
||||
flow_pred[0] = rel_vel_sensor[1] / dist_scaled
|
||||
flow_pred[1] = -rel_vel_sensor[0] / dist_scaled
|
||||
flow_pred *= state["flow_scale"]
|
||||
flow_pred = add_epsilon_sign(flow_pred, distance, epsilon)
|
||||
flow_pred = add_epsilon_sign(flow_pred, dist_scaled, epsilon)
|
||||
|
||||
return flow_pred
|
||||
|
||||
|
|
|
|||
|
|
@ -16,21 +16,21 @@ namespace sym {
|
|||
* Symbolic function: compute_airspeed_h_and_k
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* state: Matrix26_1
|
||||
* P: Matrix25_25
|
||||
* innov_var: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* H: Matrix24_1
|
||||
* K: Matrix24_1
|
||||
* H: Matrix25_1
|
||||
* K: Matrix25_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
|
||||
const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
|
||||
// Total ops: 256
|
||||
void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 26, 1>& state,
|
||||
const matrix::Matrix<Scalar, 25, 25>& P, const Scalar innov_var,
|
||||
const Scalar epsilon, matrix::Matrix<Scalar, 25, 1>* const H = nullptr,
|
||||
matrix::Matrix<Scalar, 25, 1>* const K = nullptr) {
|
||||
// Total ops: 266
|
||||
|
||||
// Input arrays
|
||||
|
||||
|
|
@ -47,7 +47,7 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
|
||||
// Output terms (2)
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 25, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
@ -59,7 +59,7 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
|
||||
matrix::Matrix<Scalar, 25, 1>& _k = (*K);
|
||||
|
||||
_k(0, 0) = _tmp6 * (-P(0, 21) * _tmp3 - P(0, 22) * _tmp4 + P(0, 3) * _tmp3 + P(0, 4) * _tmp4 +
|
||||
P(0, 5) * _tmp5);
|
||||
|
|
@ -109,6 +109,8 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
P(22, 4) * _tmp4 + P(22, 5) * _tmp5);
|
||||
_k(23, 0) = _tmp6 * (-P(23, 21) * _tmp3 - P(23, 22) * _tmp4 + P(23, 3) * _tmp3 +
|
||||
P(23, 4) * _tmp4 + P(23, 5) * _tmp5);
|
||||
_k(24, 0) = _tmp6 * (-P(24, 21) * _tmp3 - P(24, 22) * _tmp4 + P(24, 3) * _tmp3 +
|
||||
P(24, 4) * _tmp4 + P(24, 5) * _tmp5);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
|
|
|||
|
|
@ -16,8 +16,8 @@ namespace sym {
|
|||
* Symbolic function: compute_airspeed_innov_and_innov_var
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* state: Matrix26_1
|
||||
* P: Matrix25_25
|
||||
* airspeed: Scalar
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
|
|
@ -27,8 +27,8 @@ namespace sym {
|
|||
* innov_var: Scalar
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar airspeed,
|
||||
void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix<Scalar, 26, 1>& state,
|
||||
const matrix::Matrix<Scalar, 25, 25>& P, const Scalar airspeed,
|
||||
const Scalar R, const Scalar epsilon,
|
||||
Scalar* const innov = nullptr,
|
||||
Scalar* const innov_var = nullptr) {
|
||||
|
|
|
|||
|
|
@ -16,8 +16,8 @@ namespace sym {
|
|||
* Symbolic function: compute_drag_x_innov_var_and_k
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* state: Matrix26_1
|
||||
* P: Matrix25_25
|
||||
* rho: Scalar
|
||||
* cd: Scalar
|
||||
* cm: Scalar
|
||||
|
|
@ -26,14 +26,14 @@ namespace sym {
|
|||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* K: Matrix24_1
|
||||
* K: Matrix25_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar rho,
|
||||
void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 26, 1>& state,
|
||||
const matrix::Matrix<Scalar, 25, 25>& P, const Scalar rho,
|
||||
const Scalar cd, const Scalar cm, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
|
||||
matrix::Matrix<Scalar, 25, 1>* const K = nullptr) {
|
||||
// Total ops: 348
|
||||
|
||||
// Input arrays
|
||||
|
|
@ -153,7 +153,7 @@ void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
|
||||
matrix::Matrix<Scalar, 25, 1>& _k = (*K);
|
||||
|
||||
_k.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,8 +16,8 @@ namespace sym {
|
|||
* Symbolic function: compute_drag_y_innov_var_and_k
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* state: Matrix26_1
|
||||
* P: Matrix25_25
|
||||
* rho: Scalar
|
||||
* cd: Scalar
|
||||
* cm: Scalar
|
||||
|
|
@ -26,14 +26,14 @@ namespace sym {
|
|||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* K: Matrix24_1
|
||||
* K: Matrix25_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeDragYInnovVarAndK(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar rho,
|
||||
void ComputeDragYInnovVarAndK(const matrix::Matrix<Scalar, 26, 1>& state,
|
||||
const matrix::Matrix<Scalar, 25, 25>& P, const Scalar rho,
|
||||
const Scalar cd, const Scalar cm, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
|
||||
matrix::Matrix<Scalar, 25, 1>* const K = nullptr) {
|
||||
// Total ops: 348
|
||||
|
||||
// Input arrays
|
||||
|
|
@ -155,7 +155,7 @@ void ComputeDragYInnovVarAndK(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
|
||||
matrix::Matrix<Scalar, 25, 1>& _k = (*K);
|
||||
|
||||
_k.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,119 +16,130 @@ namespace sym {
|
|||
* Symbolic function: compute_flow_xy_innov_var_and_hx
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* state: Matrix26_1
|
||||
* P: Matrix25_25
|
||||
* distance: Scalar
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Matrix21
|
||||
* H: Matrix24_1
|
||||
* H: Matrix25_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar distance,
|
||||
void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 26, 1>& state,
|
||||
const matrix::Matrix<Scalar, 25, 25>& P, const Scalar distance,
|
||||
const Scalar R, const Scalar epsilon,
|
||||
matrix::Matrix<Scalar, 2, 1>* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 253
|
||||
matrix::Matrix<Scalar, 25, 1>* const H = nullptr) {
|
||||
// Total ops: 302
|
||||
|
||||
// Unused inputs
|
||||
(void)epsilon;
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (41)
|
||||
const Scalar _tmp0 = 2 * state(2, 0);
|
||||
const Scalar _tmp1 = _tmp0 * state(0, 0);
|
||||
const Scalar _tmp2 = 2 * state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp3 = -_tmp2;
|
||||
const Scalar _tmp4 = 2 * state(0, 0);
|
||||
const Scalar _tmp5 = _tmp4 * state(3, 0);
|
||||
const Scalar _tmp6 = -_tmp5;
|
||||
const Scalar _tmp7 = _tmp0 * state(1, 0);
|
||||
const Scalar _tmp8 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp9 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp10 = -_tmp9;
|
||||
const Scalar _tmp11 = _tmp10 + _tmp8;
|
||||
const Scalar _tmp12 = std::pow(state(0, 0), Scalar(2));
|
||||
// Intermediate terms (45)
|
||||
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 = -2 * _tmp0 + _tmp2;
|
||||
const Scalar _tmp4 = std::pow(distance, Scalar(-state(25, 0)));
|
||||
const Scalar _tmp5 = _tmp4 * state(24, 0);
|
||||
const Scalar _tmp6 = _tmp3 * _tmp5;
|
||||
const Scalar _tmp7 = 2 * state(0, 0);
|
||||
const Scalar _tmp8 = _tmp7 * state(2, 0);
|
||||
const Scalar _tmp9 = 2 * state(1, 0);
|
||||
const Scalar _tmp10 = _tmp9 * state(3, 0);
|
||||
const Scalar _tmp11 = 2 * state(2, 0) * state(3, 0);
|
||||
const Scalar _tmp12 = _tmp9 * state(0, 0);
|
||||
const Scalar _tmp13 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp14 = -_tmp12 + _tmp13;
|
||||
const Scalar _tmp15 =
|
||||
Scalar(1.0) /
|
||||
(distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
|
||||
const Scalar _tmp16 = _tmp15 * state(24, 0);
|
||||
const Scalar _tmp17 = _tmp16 * (state(4, 0) * (_tmp11 + _tmp14) + state(5, 0) * (_tmp6 - _tmp7) +
|
||||
state(6, 0) * (_tmp1 + _tmp3));
|
||||
const Scalar _tmp18 = 1 - 2 * _tmp8;
|
||||
const Scalar _tmp19 = _tmp18 - 2 * _tmp9;
|
||||
const Scalar _tmp20 = _tmp6 + _tmp7;
|
||||
const Scalar _tmp21 = _tmp0 * state(3, 0);
|
||||
const Scalar _tmp22 = _tmp4 * state(1, 0);
|
||||
const Scalar _tmp23 = _tmp21 + _tmp22;
|
||||
const Scalar _tmp24 = _tmp20 * state(4, 0) + _tmp23 * state(6, 0);
|
||||
const Scalar _tmp25 = _tmp15 * (_tmp19 * state(5, 0) + _tmp24);
|
||||
const Scalar _tmp26 = _tmp16 * _tmp19;
|
||||
const Scalar _tmp27 = _tmp16 * (state(4, 0) * (_tmp1 + _tmp2) + state(5, 0) * (_tmp21 - _tmp22) +
|
||||
state(6, 0) * (_tmp11 + _tmp12 - _tmp13));
|
||||
const Scalar _tmp28 = _tmp16 * _tmp20;
|
||||
const Scalar _tmp29 = _tmp16 * _tmp23;
|
||||
const Scalar _tmp30 = -2 * _tmp13 + _tmp18;
|
||||
const Scalar _tmp31 = _tmp16 * _tmp30;
|
||||
const Scalar _tmp32 = -_tmp1;
|
||||
const Scalar _tmp33 = -_tmp8;
|
||||
const Scalar _tmp14 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp15 = -_tmp0;
|
||||
const Scalar _tmp16 = _tmp14 + _tmp15;
|
||||
const Scalar _tmp17 = _tmp5 * (state(4, 0) * (_tmp10 + _tmp8) + state(5, 0) * (_tmp11 - _tmp12) +
|
||||
state(6, 0) * (_tmp1 - _tmp13 + _tmp16));
|
||||
const Scalar _tmp18 = std::log(distance);
|
||||
const Scalar _tmp19 = _tmp7 * state(3, 0);
|
||||
const Scalar _tmp20 = -_tmp19;
|
||||
const Scalar _tmp21 = _tmp9 * state(2, 0);
|
||||
const Scalar _tmp22 = _tmp20 + _tmp21;
|
||||
const Scalar _tmp23 = _tmp11 + _tmp12;
|
||||
const Scalar _tmp24 = _tmp22 * state(4, 0) + _tmp23 * state(6, 0);
|
||||
const Scalar _tmp25 = _tmp4 * (_tmp24 + _tmp3 * state(5, 0));
|
||||
const Scalar _tmp26 = _tmp18 * _tmp25 * state(24, 0);
|
||||
const Scalar _tmp27 = _tmp22 * _tmp5;
|
||||
const Scalar _tmp28 = -_tmp10;
|
||||
const Scalar _tmp29 = -_tmp14;
|
||||
const Scalar _tmp30 = _tmp5 * (state(4, 0) * (_tmp1 + _tmp13 + _tmp15 + _tmp29) +
|
||||
state(5, 0) * (_tmp20 - _tmp21) + state(6, 0) * (_tmp28 + _tmp8));
|
||||
const Scalar _tmp31 = _tmp23 * _tmp5;
|
||||
const Scalar _tmp32 = -_tmp8;
|
||||
const Scalar _tmp33 = -_tmp1 + _tmp13;
|
||||
const Scalar _tmp34 =
|
||||
_tmp16 * (state(4, 0) * (_tmp3 + _tmp32) + state(5, 0) * (-_tmp21 + _tmp22) +
|
||||
state(6, 0) * (_tmp14 + _tmp33 + _tmp9));
|
||||
const Scalar _tmp35 = _tmp2 + _tmp32;
|
||||
const Scalar _tmp36 = _tmp16 * _tmp35;
|
||||
const Scalar _tmp37 = _tmp5 + _tmp7;
|
||||
const Scalar _tmp38 = _tmp16 * _tmp37;
|
||||
const Scalar _tmp39 = _tmp16 * (_tmp24 + state(5, 0) * (_tmp10 + _tmp12 + _tmp13 + _tmp33));
|
||||
const Scalar _tmp40 =
|
||||
_tmp15 * (_tmp30 * state(4, 0) + _tmp35 * state(6, 0) + _tmp37 * state(5, 0));
|
||||
_tmp5 * (state(4, 0) * (_tmp28 + _tmp32) + state(5, 0) * (-_tmp11 + _tmp12) +
|
||||
state(6, 0) * (_tmp0 + _tmp29 + _tmp33));
|
||||
const Scalar _tmp35 = _tmp5 * (_tmp24 + state(5, 0) * (_tmp16 + _tmp33));
|
||||
const Scalar _tmp36 = _tmp10 + _tmp32;
|
||||
const Scalar _tmp37 = _tmp19 + _tmp21;
|
||||
const Scalar _tmp38 = -2 * _tmp13 + _tmp2;
|
||||
const Scalar _tmp39 = _tmp36 * state(6, 0) + _tmp37 * state(5, 0) + _tmp38 * state(4, 0);
|
||||
const Scalar _tmp40 = _tmp18 * _tmp39 * _tmp5;
|
||||
const Scalar _tmp41 = _tmp38 * _tmp5;
|
||||
const Scalar _tmp42 = _tmp37 * _tmp5;
|
||||
const Scalar _tmp43 = _tmp36 * _tmp5;
|
||||
const Scalar _tmp44 = _tmp39 * _tmp4;
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
matrix::Matrix<Scalar, 2, 1>& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var(0, 0) = R +
|
||||
_tmp17 * (P(0, 2) * _tmp27 + P(2, 2) * _tmp17 + P(23, 2) * _tmp25 +
|
||||
P(3, 2) * _tmp28 + P(4, 2) * _tmp26 + P(5, 2) * _tmp29) +
|
||||
_tmp25 * (P(0, 23) * _tmp27 + P(2, 23) * _tmp17 + P(23, 23) * _tmp25 +
|
||||
P(3, 23) * _tmp28 + P(4, 23) * _tmp26 + P(5, 23) * _tmp29) +
|
||||
_tmp26 * (P(0, 4) * _tmp27 + P(2, 4) * _tmp17 + P(23, 4) * _tmp25 +
|
||||
P(3, 4) * _tmp28 + P(4, 4) * _tmp26 + P(5, 4) * _tmp29) +
|
||||
_tmp27 * (P(0, 0) * _tmp27 + P(2, 0) * _tmp17 + P(23, 0) * _tmp25 +
|
||||
P(3, 0) * _tmp28 + P(4, 0) * _tmp26 + P(5, 0) * _tmp29) +
|
||||
_tmp28 * (P(0, 3) * _tmp27 + P(2, 3) * _tmp17 + P(23, 3) * _tmp25 +
|
||||
P(3, 3) * _tmp28 + P(4, 3) * _tmp26 + P(5, 3) * _tmp29) +
|
||||
_tmp29 * (P(0, 5) * _tmp27 + P(2, 5) * _tmp17 + P(23, 5) * _tmp25 +
|
||||
P(3, 5) * _tmp28 + P(4, 5) * _tmp26 + P(5, 5) * _tmp29);
|
||||
_innov_var(1, 0) = R -
|
||||
_tmp31 * (-P(1, 3) * _tmp34 - P(2, 3) * _tmp39 - P(23, 3) * _tmp40 -
|
||||
P(3, 3) * _tmp31 - P(4, 3) * _tmp38 - P(5, 3) * _tmp36) -
|
||||
_tmp34 * (-P(1, 1) * _tmp34 - P(2, 1) * _tmp39 - P(23, 1) * _tmp40 -
|
||||
P(3, 1) * _tmp31 - P(4, 1) * _tmp38 - P(5, 1) * _tmp36) -
|
||||
_tmp36 * (-P(1, 5) * _tmp34 - P(2, 5) * _tmp39 - P(23, 5) * _tmp40 -
|
||||
P(3, 5) * _tmp31 - P(4, 5) * _tmp38 - P(5, 5) * _tmp36) -
|
||||
_tmp38 * (-P(1, 4) * _tmp34 - P(2, 4) * _tmp39 - P(23, 4) * _tmp40 -
|
||||
P(3, 4) * _tmp31 - P(4, 4) * _tmp38 - P(5, 4) * _tmp36) -
|
||||
_tmp39 * (-P(1, 2) * _tmp34 - P(2, 2) * _tmp39 - P(23, 2) * _tmp40 -
|
||||
P(3, 2) * _tmp31 - P(4, 2) * _tmp38 - P(5, 2) * _tmp36) -
|
||||
_tmp40 * (-P(1, 23) * _tmp34 - P(2, 23) * _tmp39 - P(23, 23) * _tmp40 -
|
||||
P(3, 23) * _tmp31 - P(4, 23) * _tmp38 - P(5, 23) * _tmp36);
|
||||
_innov_var(0, 0) =
|
||||
R +
|
||||
_tmp17 * (P(0, 0) * _tmp17 + P(2, 0) * _tmp30 + P(23, 0) * _tmp25 - P(24, 0) * _tmp26 +
|
||||
P(3, 0) * _tmp27 + P(4, 0) * _tmp6 + P(5, 0) * _tmp31) +
|
||||
_tmp25 * (P(0, 23) * _tmp17 + P(2, 23) * _tmp30 + P(23, 23) * _tmp25 - P(24, 23) * _tmp26 +
|
||||
P(3, 23) * _tmp27 + P(4, 23) * _tmp6 + P(5, 23) * _tmp31) -
|
||||
_tmp26 * (P(0, 24) * _tmp17 + P(2, 24) * _tmp30 + P(23, 24) * _tmp25 - P(24, 24) * _tmp26 +
|
||||
P(3, 24) * _tmp27 + P(4, 24) * _tmp6 + P(5, 24) * _tmp31) +
|
||||
_tmp27 * (P(0, 3) * _tmp17 + P(2, 3) * _tmp30 + P(23, 3) * _tmp25 - P(24, 3) * _tmp26 +
|
||||
P(3, 3) * _tmp27 + P(4, 3) * _tmp6 + P(5, 3) * _tmp31) +
|
||||
_tmp30 * (P(0, 2) * _tmp17 + P(2, 2) * _tmp30 + P(23, 2) * _tmp25 - P(24, 2) * _tmp26 +
|
||||
P(3, 2) * _tmp27 + P(4, 2) * _tmp6 + P(5, 2) * _tmp31) +
|
||||
_tmp31 * (P(0, 5) * _tmp17 + P(2, 5) * _tmp30 + P(23, 5) * _tmp25 - P(24, 5) * _tmp26 +
|
||||
P(3, 5) * _tmp27 + P(4, 5) * _tmp6 + P(5, 5) * _tmp31) +
|
||||
_tmp6 * (P(0, 4) * _tmp17 + P(2, 4) * _tmp30 + P(23, 4) * _tmp25 - P(24, 4) * _tmp26 +
|
||||
P(3, 4) * _tmp27 + P(4, 4) * _tmp6 + P(5, 4) * _tmp31);
|
||||
_innov_var(1, 0) =
|
||||
R -
|
||||
_tmp34 * (-P(1, 1) * _tmp34 - P(2, 1) * _tmp35 - P(23, 1) * _tmp44 + P(24, 1) * _tmp40 -
|
||||
P(3, 1) * _tmp41 - P(4, 1) * _tmp42 - P(5, 1) * _tmp43) -
|
||||
_tmp35 * (-P(1, 2) * _tmp34 - P(2, 2) * _tmp35 - P(23, 2) * _tmp44 + P(24, 2) * _tmp40 -
|
||||
P(3, 2) * _tmp41 - P(4, 2) * _tmp42 - P(5, 2) * _tmp43) +
|
||||
_tmp40 * (-P(1, 24) * _tmp34 - P(2, 24) * _tmp35 - P(23, 24) * _tmp44 + P(24, 24) * _tmp40 -
|
||||
P(3, 24) * _tmp41 - P(4, 24) * _tmp42 - P(5, 24) * _tmp43) -
|
||||
_tmp41 * (-P(1, 3) * _tmp34 - P(2, 3) * _tmp35 - P(23, 3) * _tmp44 + P(24, 3) * _tmp40 -
|
||||
P(3, 3) * _tmp41 - P(4, 3) * _tmp42 - P(5, 3) * _tmp43) -
|
||||
_tmp42 * (-P(1, 4) * _tmp34 - P(2, 4) * _tmp35 - P(23, 4) * _tmp44 + P(24, 4) * _tmp40 -
|
||||
P(3, 4) * _tmp41 - P(4, 4) * _tmp42 - P(5, 4) * _tmp43) -
|
||||
_tmp43 * (-P(1, 5) * _tmp34 - P(2, 5) * _tmp35 - P(23, 5) * _tmp44 + P(24, 5) * _tmp40 -
|
||||
P(3, 5) * _tmp41 - P(4, 5) * _tmp42 - P(5, 5) * _tmp43) -
|
||||
_tmp44 * (-P(1, 23) * _tmp34 - P(2, 23) * _tmp35 - P(23, 23) * _tmp44 + P(24, 23) * _tmp40 -
|
||||
P(3, 23) * _tmp41 - P(4, 23) * _tmp42 - P(5, 23) * _tmp43);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 25, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp27;
|
||||
_h(2, 0) = _tmp17;
|
||||
_h(3, 0) = _tmp28;
|
||||
_h(4, 0) = _tmp26;
|
||||
_h(5, 0) = _tmp29;
|
||||
_h(0, 0) = _tmp17;
|
||||
_h(2, 0) = _tmp30;
|
||||
_h(3, 0) = _tmp27;
|
||||
_h(4, 0) = _tmp6;
|
||||
_h(5, 0) = _tmp31;
|
||||
_h(23, 0) = _tmp25;
|
||||
_h(24, 0) = -_tmp26;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
|
|
|||
|
|
@ -16,87 +16,94 @@ namespace sym {
|
|||
* Symbolic function: compute_flow_y_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* state: Matrix26_1
|
||||
* P: Matrix25_25
|
||||
* distance: Scalar
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
* H: Matrix25_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar distance,
|
||||
void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 26, 1>& state,
|
||||
const matrix::Matrix<Scalar, 25, 25>& P, const Scalar distance,
|
||||
const Scalar R, const Scalar epsilon,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 148
|
||||
matrix::Matrix<Scalar, 25, 1>* const H = nullptr) {
|
||||
// Total ops: 167
|
||||
|
||||
// Unused inputs
|
||||
(void)epsilon;
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (24)
|
||||
const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp2 = -2 * _tmp0 - 2 * _tmp1 + 1;
|
||||
const Scalar _tmp3 =
|
||||
Scalar(1.0) /
|
||||
(distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
|
||||
const Scalar _tmp4 = _tmp3 * state(24, 0);
|
||||
const Scalar _tmp5 = _tmp2 * _tmp4;
|
||||
const Scalar _tmp6 = 2 * state(0, 0);
|
||||
const Scalar _tmp7 = -_tmp6 * state(2, 0);
|
||||
const Scalar _tmp8 = 2 * state(1, 0);
|
||||
const Scalar _tmp9 = _tmp8 * state(3, 0);
|
||||
const Scalar _tmp10 = 2 * state(2, 0) * state(3, 0);
|
||||
const Scalar _tmp11 = _tmp8 * state(0, 0);
|
||||
const Scalar _tmp12 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp13 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp14 = -_tmp0 + _tmp1;
|
||||
const Scalar _tmp15 = _tmp4 * (state(4, 0) * (_tmp7 - _tmp9) + state(5, 0) * (-_tmp10 + _tmp11) +
|
||||
state(6, 0) * (-_tmp12 + _tmp13 + _tmp14));
|
||||
const Scalar _tmp16 = _tmp7 + _tmp9;
|
||||
const Scalar _tmp17 = _tmp16 * _tmp4;
|
||||
const Scalar _tmp18 = _tmp6 * state(3, 0);
|
||||
const Scalar _tmp19 = _tmp8 * state(2, 0);
|
||||
const Scalar _tmp20 = _tmp18 + _tmp19;
|
||||
const Scalar _tmp21 = _tmp20 * _tmp4;
|
||||
const Scalar _tmp22 =
|
||||
_tmp4 * (state(4, 0) * (-_tmp18 + _tmp19) + state(5, 0) * (_tmp12 - _tmp13 + _tmp14) +
|
||||
state(6, 0) * (_tmp10 + _tmp11));
|
||||
const Scalar _tmp23 = _tmp3 * (_tmp16 * state(6, 0) + _tmp2 * state(4, 0) + _tmp20 * state(5, 0));
|
||||
// Intermediate terms (25)
|
||||
const Scalar _tmp0 = 2 * state(0, 0);
|
||||
const Scalar _tmp1 = -_tmp0 * state(2, 0);
|
||||
const Scalar _tmp2 = 2 * state(3, 0);
|
||||
const Scalar _tmp3 = _tmp2 * state(1, 0);
|
||||
const Scalar _tmp4 = _tmp2 * state(2, 0);
|
||||
const Scalar _tmp5 = _tmp0 * state(1, 0);
|
||||
const Scalar _tmp6 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp7 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp8 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp9 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp10 = -_tmp8 + _tmp9;
|
||||
const Scalar _tmp11 = std::pow(distance, Scalar(-state(25, 0)));
|
||||
const Scalar _tmp12 = _tmp11 * state(24, 0);
|
||||
const Scalar _tmp13 = _tmp12 * (state(4, 0) * (_tmp1 - _tmp3) + state(5, 0) * (-_tmp4 + _tmp5) +
|
||||
state(6, 0) * (_tmp10 - _tmp6 + _tmp7));
|
||||
const Scalar _tmp14 = _tmp2 * state(0, 0);
|
||||
const Scalar _tmp15 = 2 * state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp16 =
|
||||
_tmp12 * (state(4, 0) * (-_tmp14 + _tmp15) + state(5, 0) * (_tmp10 + _tmp6 - _tmp7) +
|
||||
state(6, 0) * (_tmp4 + _tmp5));
|
||||
const Scalar _tmp17 = _tmp1 + _tmp3;
|
||||
const Scalar _tmp18 = _tmp14 + _tmp15;
|
||||
const Scalar _tmp19 = -2 * _tmp8 - 2 * _tmp9 + 1;
|
||||
const Scalar _tmp20 =
|
||||
_tmp11 * (_tmp17 * state(6, 0) + _tmp18 * state(5, 0) + _tmp19 * state(4, 0));
|
||||
const Scalar _tmp21 = _tmp20 * state(24, 0) * std::log(distance);
|
||||
const Scalar _tmp22 = _tmp12 * _tmp19;
|
||||
const Scalar _tmp23 = _tmp12 * _tmp18;
|
||||
const Scalar _tmp24 = _tmp12 * _tmp17;
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R -
|
||||
_tmp15 * (-P(1, 1) * _tmp15 - P(2, 1) * _tmp22 - P(23, 1) * _tmp23 -
|
||||
P(3, 1) * _tmp5 - P(4, 1) * _tmp21 - P(5, 1) * _tmp17) -
|
||||
_tmp17 * (-P(1, 5) * _tmp15 - P(2, 5) * _tmp22 - P(23, 5) * _tmp23 -
|
||||
P(3, 5) * _tmp5 - P(4, 5) * _tmp21 - P(5, 5) * _tmp17) -
|
||||
_tmp21 * (-P(1, 4) * _tmp15 - P(2, 4) * _tmp22 - P(23, 4) * _tmp23 -
|
||||
P(3, 4) * _tmp5 - P(4, 4) * _tmp21 - P(5, 4) * _tmp17) -
|
||||
_tmp22 * (-P(1, 2) * _tmp15 - P(2, 2) * _tmp22 - P(23, 2) * _tmp23 -
|
||||
P(3, 2) * _tmp5 - P(4, 2) * _tmp21 - P(5, 2) * _tmp17) -
|
||||
_tmp23 * (-P(1, 23) * _tmp15 - P(2, 23) * _tmp22 - P(23, 23) * _tmp23 -
|
||||
P(3, 23) * _tmp5 - P(4, 23) * _tmp21 - P(5, 23) * _tmp17) -
|
||||
_tmp5 * (-P(1, 3) * _tmp15 - P(2, 3) * _tmp22 - P(23, 3) * _tmp23 -
|
||||
P(3, 3) * _tmp5 - P(4, 3) * _tmp21 - P(5, 3) * _tmp17);
|
||||
_innov_var =
|
||||
R -
|
||||
_tmp13 * (-P(1, 1) * _tmp13 - P(2, 1) * _tmp16 - P(23, 1) * _tmp20 + P(24, 1) * _tmp21 -
|
||||
P(3, 1) * _tmp22 - P(4, 1) * _tmp23 - P(5, 1) * _tmp24) -
|
||||
_tmp16 * (-P(1, 2) * _tmp13 - P(2, 2) * _tmp16 - P(23, 2) * _tmp20 + P(24, 2) * _tmp21 -
|
||||
P(3, 2) * _tmp22 - P(4, 2) * _tmp23 - P(5, 2) * _tmp24) -
|
||||
_tmp20 * (-P(1, 23) * _tmp13 - P(2, 23) * _tmp16 - P(23, 23) * _tmp20 + P(24, 23) * _tmp21 -
|
||||
P(3, 23) * _tmp22 - P(4, 23) * _tmp23 - P(5, 23) * _tmp24) +
|
||||
_tmp21 * (-P(1, 24) * _tmp13 - P(2, 24) * _tmp16 - P(23, 24) * _tmp20 + P(24, 24) * _tmp21 -
|
||||
P(3, 24) * _tmp22 - P(4, 24) * _tmp23 - P(5, 24) * _tmp24) -
|
||||
_tmp22 * (-P(1, 3) * _tmp13 - P(2, 3) * _tmp16 - P(23, 3) * _tmp20 + P(24, 3) * _tmp21 -
|
||||
P(3, 3) * _tmp22 - P(4, 3) * _tmp23 - P(5, 3) * _tmp24) -
|
||||
_tmp23 * (-P(1, 4) * _tmp13 - P(2, 4) * _tmp16 - P(23, 4) * _tmp20 + P(24, 4) * _tmp21 -
|
||||
P(3, 4) * _tmp22 - P(4, 4) * _tmp23 - P(5, 4) * _tmp24) -
|
||||
_tmp24 * (-P(1, 5) * _tmp13 - P(2, 5) * _tmp16 - P(23, 5) * _tmp20 + P(24, 5) * _tmp21 -
|
||||
P(3, 5) * _tmp22 - P(4, 5) * _tmp23 - P(5, 5) * _tmp24);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 25, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(1, 0) = -_tmp15;
|
||||
_h(2, 0) = -_tmp22;
|
||||
_h(3, 0) = -_tmp5;
|
||||
_h(4, 0) = -_tmp21;
|
||||
_h(5, 0) = -_tmp17;
|
||||
_h(23, 0) = -_tmp23;
|
||||
_h(1, 0) = -_tmp13;
|
||||
_h(2, 0) = -_tmp16;
|
||||
_h(3, 0) = -_tmp22;
|
||||
_h(4, 0) = -_tmp23;
|
||||
_h(5, 0) = -_tmp24;
|
||||
_h(23, 0) = -_tmp20;
|
||||
_h(24, 0) = _tmp21;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
|
|
|||
|
|
@ -16,8 +16,8 @@ namespace sym {
|
|||
* Symbolic function: compute_gnss_yaw_pred_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* state: Matrix26_1
|
||||
* P: Matrix25_25
|
||||
* antenna_yaw_offset: Scalar
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
|
|
@ -25,15 +25,15 @@ namespace sym {
|
|||
* Outputs:
|
||||
* meas_pred: Scalar
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
* H: Matrix25_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P,
|
||||
void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 26, 1>& state,
|
||||
const matrix::Matrix<Scalar, 25, 25>& P,
|
||||
const Scalar antenna_yaw_offset, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const meas_pred = nullptr,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
matrix::Matrix<Scalar, 25, 1>* const H = nullptr) {
|
||||
// Total ops: 95
|
||||
|
||||
// Input arrays
|
||||
|
|
@ -88,7 +88,7 @@ void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 25, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,8 +16,8 @@ namespace sym {
|
|||
* Symbolic function: compute_gravity_innov_var_and_k_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* state: Matrix26_1
|
||||
* P: Matrix25_25
|
||||
* meas: Matrix31
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
|
|
@ -25,21 +25,21 @@ namespace sym {
|
|||
* Outputs:
|
||||
* innov: Matrix31
|
||||
* innov_var: Matrix31
|
||||
* Kx: Matrix24_1
|
||||
* Ky: Matrix24_1
|
||||
* Kz: Matrix24_1
|
||||
* Kx: Matrix25_1
|
||||
* Ky: Matrix25_1
|
||||
* Kz: Matrix25_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P,
|
||||
void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 26, 1>& state,
|
||||
const matrix::Matrix<Scalar, 25, 25>& P,
|
||||
const matrix::Matrix<Scalar, 3, 1>& meas, const Scalar R,
|
||||
const Scalar epsilon,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov = nullptr,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const Kx = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const Ky = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const Kz = nullptr) {
|
||||
// Total ops: 373
|
||||
matrix::Matrix<Scalar, 25, 1>* const Kx = nullptr,
|
||||
matrix::Matrix<Scalar, 25, 1>* const Ky = nullptr,
|
||||
matrix::Matrix<Scalar, 25, 1>* const Kz = nullptr) {
|
||||
// Total ops: 385
|
||||
|
||||
// Input arrays
|
||||
|
||||
|
|
@ -103,7 +103,7 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
}
|
||||
|
||||
if (Kx != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _kx = (*Kx);
|
||||
matrix::Matrix<Scalar, 25, 1>& _kx = (*Kx);
|
||||
|
||||
_kx(0, 0) = _tmp28 * (P(0, 1) * _tmp13 + P(0, 2) * _tmp14);
|
||||
_kx(1, 0) = _tmp28 * (P(1, 2) * _tmp14 + _tmp16);
|
||||
|
|
@ -129,10 +129,11 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
_kx(21, 0) = _tmp28 * (P(21, 1) * _tmp13 + P(21, 2) * _tmp14);
|
||||
_kx(22, 0) = _tmp28 * (P(22, 1) * _tmp13 + P(22, 2) * _tmp14);
|
||||
_kx(23, 0) = _tmp28 * (P(23, 1) * _tmp13 + P(23, 2) * _tmp14);
|
||||
_kx(24, 0) = _tmp28 * (P(24, 1) * _tmp13 + P(24, 2) * _tmp14);
|
||||
}
|
||||
|
||||
if (Ky != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _ky = (*Ky);
|
||||
matrix::Matrix<Scalar, 25, 1>& _ky = (*Ky);
|
||||
|
||||
_ky(0, 0) = _tmp29 * (P(0, 2) * _tmp19 + _tmp21);
|
||||
_ky(1, 0) = _tmp29 * (P(1, 0) * _tmp18 + P(1, 2) * _tmp19);
|
||||
|
|
@ -158,10 +159,11 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
_ky(21, 0) = _tmp29 * (P(21, 0) * _tmp18 + P(21, 2) * _tmp19);
|
||||
_ky(22, 0) = _tmp29 * (P(22, 0) * _tmp18 + P(22, 2) * _tmp19);
|
||||
_ky(23, 0) = _tmp29 * (P(23, 0) * _tmp18 + P(23, 2) * _tmp19);
|
||||
_ky(24, 0) = _tmp29 * (P(24, 0) * _tmp18 + P(24, 2) * _tmp19);
|
||||
}
|
||||
|
||||
if (Kz != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _kz = (*Kz);
|
||||
matrix::Matrix<Scalar, 25, 1>& _kz = (*Kz);
|
||||
|
||||
_kz(0, 0) = _tmp30 * (P(0, 1) * _tmp24 + _tmp26);
|
||||
_kz(1, 0) = _tmp30 * (P(1, 0) * _tmp23 + _tmp25);
|
||||
|
|
@ -187,6 +189,7 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
_kz(21, 0) = _tmp30 * (P(21, 0) * _tmp23 + P(21, 1) * _tmp24);
|
||||
_kz(22, 0) = _tmp30 * (P(22, 0) * _tmp23 + P(22, 1) * _tmp24);
|
||||
_kz(23, 0) = _tmp30 * (P(23, 0) * _tmp23 + P(23, 1) * _tmp24);
|
||||
_kz(24, 0) = _tmp30 * (P(24, 0) * _tmp23 + P(24, 1) * _tmp24);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
|
|
|||
|
|
@ -16,22 +16,22 @@ namespace sym {
|
|||
* Symbolic function: compute_mag_declination_pred_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* state: Matrix26_1
|
||||
* P: Matrix25_25
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* pred: Scalar
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
* H: Matrix25_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 26, 1>& state,
|
||||
const matrix::Matrix<Scalar, 25, 25>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const pred = nullptr,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
matrix::Matrix<Scalar, 25, 1>* const H = nullptr) {
|
||||
// Total ops: 22
|
||||
|
||||
// Input arrays
|
||||
|
|
@ -59,7 +59,7 @@ void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>&
|
|||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 25, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,8 +16,8 @@ namespace sym {
|
|||
* Symbolic function: compute_mag_innov_innov_var_and_hx
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* state: Matrix26_1
|
||||
* P: Matrix25_25
|
||||
* meas: Matrix31
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
|
|
@ -25,16 +25,16 @@ namespace sym {
|
|||
* Outputs:
|
||||
* innov: Matrix31
|
||||
* innov_var: Matrix31
|
||||
* Hx: Matrix24_1
|
||||
* Hx: Matrix25_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P,
|
||||
void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 26, 1>& state,
|
||||
const matrix::Matrix<Scalar, 25, 25>& P,
|
||||
const matrix::Matrix<Scalar, 3, 1>& meas, const Scalar R,
|
||||
const Scalar epsilon,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov = nullptr,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const Hx = nullptr) {
|
||||
matrix::Matrix<Scalar, 25, 1>* const Hx = nullptr) {
|
||||
// Total ops: 314
|
||||
|
||||
// Unused inputs
|
||||
|
|
@ -146,7 +146,7 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
}
|
||||
|
||||
if (Hx != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _hx = (*Hx);
|
||||
matrix::Matrix<Scalar, 25, 1>& _hx = (*Hx);
|
||||
|
||||
_hx.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,20 +16,20 @@ namespace sym {
|
|||
* Symbolic function: compute_mag_y_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* state: Matrix26_1
|
||||
* P: Matrix25_25
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
* H: Matrix25_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 26, 1>& state,
|
||||
const matrix::Matrix<Scalar, 25, 25>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
matrix::Matrix<Scalar, 25, 1>* const H = nullptr) {
|
||||
// Total ops: 110
|
||||
|
||||
// Unused inputs
|
||||
|
|
@ -78,7 +78,7 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 25, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,20 +16,20 @@ namespace sym {
|
|||
* Symbolic function: compute_mag_z_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* state: Matrix26_1
|
||||
* P: Matrix25_25
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
* H: Matrix25_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 26, 1>& state,
|
||||
const matrix::Matrix<Scalar, 25, 25>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
matrix::Matrix<Scalar, 25, 1>* const H = nullptr) {
|
||||
// Total ops: 110
|
||||
|
||||
// Unused inputs
|
||||
|
|
@ -78,7 +78,7 @@ void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 25, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,21 +16,21 @@ namespace sym {
|
|||
* Symbolic function: compute_sideslip_h_and_k
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* state: Matrix26_1
|
||||
* P: Matrix25_25
|
||||
* innov_var: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* H: Matrix24_1
|
||||
* K: Matrix24_1
|
||||
* H: Matrix25_1
|
||||
* K: Matrix25_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
|
||||
const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
|
||||
// Total ops: 485
|
||||
void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 26, 1>& state,
|
||||
const matrix::Matrix<Scalar, 25, 25>& P, const Scalar innov_var,
|
||||
const Scalar epsilon, matrix::Matrix<Scalar, 25, 1>* const H = nullptr,
|
||||
matrix::Matrix<Scalar, 25, 1>* const K = nullptr) {
|
||||
// Total ops: 501
|
||||
|
||||
// Input arrays
|
||||
|
||||
|
|
@ -42,20 +42,20 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
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 _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 = _tmp5 * state(2, 0);
|
||||
const Scalar _tmp11 = _tmp7 * state(0, 0);
|
||||
const Scalar _tmp12 = -_tmp11;
|
||||
const Scalar _tmp13 = _tmp7 * state(3, 0);
|
||||
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 = 2 * state(2, 0) * state(3, 0);
|
||||
const Scalar _tmp19 = _tmp7 * state(0, 0);
|
||||
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;
|
||||
|
|
@ -90,7 +90,7 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
|
||||
// Output terms (2)
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 25, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
@ -105,7 +105,7 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
|
||||
matrix::Matrix<Scalar, 25, 1>& _k = (*K);
|
||||
|
||||
_k(0, 0) =
|
||||
_tmp45 * (P(0, 0) * _tmp24 - P(0, 1) * _tmp34 + P(0, 2) * _tmp35 + P(0, 21) * _tmp43 +
|
||||
|
|
@ -179,6 +179,9 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
_k(23, 0) =
|
||||
_tmp45 * (P(23, 0) * _tmp24 - P(23, 1) * _tmp34 + P(23, 2) * _tmp35 + P(23, 21) * _tmp43 +
|
||||
P(23, 22) * _tmp44 + P(23, 3) * _tmp38 + P(23, 4) * _tmp41 + P(23, 5) * _tmp42);
|
||||
_k(24, 0) =
|
||||
_tmp45 * (P(24, 0) * _tmp24 - P(24, 1) * _tmp34 + P(24, 2) * _tmp35 + P(24, 21) * _tmp43 +
|
||||
P(24, 22) * _tmp44 + P(24, 3) * _tmp38 + P(24, 4) * _tmp41 + P(24, 5) * _tmp42);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
|
|
|||
|
|
@ -16,8 +16,8 @@ namespace sym {
|
|||
* Symbolic function: compute_sideslip_innov_and_innov_var
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* state: Matrix26_1
|
||||
* P: Matrix25_25
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
|
|
@ -26,8 +26,8 @@ namespace sym {
|
|||
* innov_var: Scalar
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 26, 1>& state,
|
||||
const matrix::Matrix<Scalar, 25, 25>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov = nullptr,
|
||||
Scalar* const innov_var = nullptr) {
|
||||
// Total ops: 235
|
||||
|
|
|
|||
|
|
@ -16,20 +16,20 @@ namespace sym {
|
|||
* Symbolic function: compute_yaw_312_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* state: Matrix26_1
|
||||
* P: Matrix25_25
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
* H: Matrix25_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeYaw312InnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
void ComputeYaw312InnovVarAndH(const matrix::Matrix<Scalar, 26, 1>& state,
|
||||
const matrix::Matrix<Scalar, 25, 25>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
matrix::Matrix<Scalar, 25, 1>* const H = nullptr) {
|
||||
// Total ops: 53
|
||||
|
||||
// Input arrays
|
||||
|
|
@ -63,7 +63,7 @@ void ComputeYaw312InnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 25, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,20 +16,20 @@ namespace sym {
|
|||
* Symbolic function: compute_yaw_312_innov_var_and_h_alternate
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* state: Matrix26_1
|
||||
* P: Matrix25_25
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
* H: Matrix25_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 26, 1>& state,
|
||||
const matrix::Matrix<Scalar, 25, 25>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
matrix::Matrix<Scalar, 25, 1>* const H = nullptr) {
|
||||
// Total ops: 57
|
||||
|
||||
// Input arrays
|
||||
|
|
@ -63,7 +63,7 @@ void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 25, 1>& sta
|
|||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 25, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,20 +16,20 @@ namespace sym {
|
|||
* Symbolic function: compute_yaw_321_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* state: Matrix26_1
|
||||
* P: Matrix25_25
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
* H: Matrix25_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeYaw321InnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
void ComputeYaw321InnovVarAndH(const matrix::Matrix<Scalar, 26, 1>& state,
|
||||
const matrix::Matrix<Scalar, 25, 25>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
matrix::Matrix<Scalar, 25, 1>* const H = nullptr) {
|
||||
// Total ops: 53
|
||||
|
||||
// Input arrays
|
||||
|
|
@ -63,7 +63,7 @@ void ComputeYaw321InnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
|||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 25, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,20 +16,20 @@ namespace sym {
|
|||
* Symbolic function: compute_yaw_321_innov_var_and_h_alternate
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* state: Matrix26_1
|
||||
* P: Matrix25_25
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
* H: Matrix25_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 26, 1>& state,
|
||||
const matrix::Matrix<Scalar, 25, 25>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
matrix::Matrix<Scalar, 25, 1>* const H = nullptr) {
|
||||
// Total ops: 57
|
||||
|
||||
// Input arrays
|
||||
|
|
@ -63,7 +63,7 @@ void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 25, 1>& sta
|
|||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 25, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -19,9 +19,10 @@ struct StateSample {
|
|||
matrix::Vector3<float> mag_B{};
|
||||
matrix::Vector2<float> wind_vel{};
|
||||
float flow_scale{};
|
||||
float flow_exp{};
|
||||
|
||||
matrix::Vector<float, 25> Data() const {
|
||||
matrix::Vector<float, 25> state;
|
||||
matrix::Vector<float, 26> Data() const {
|
||||
matrix::Vector<float, 26> state;
|
||||
state.slice<4, 1>(0, 0) = quat_nominal;
|
||||
state.slice<3, 1>(4, 0) = vel;
|
||||
state.slice<3, 1>(7, 0) = pos;
|
||||
|
|
@ -31,15 +32,16 @@ struct StateSample {
|
|||
state.slice<3, 1>(19, 0) = mag_B;
|
||||
state.slice<2, 1>(22, 0) = wind_vel;
|
||||
state.slice<1, 1>(24, 0) = flow_scale;
|
||||
state.slice<1, 1>(25, 0) = flow_exp;
|
||||
return state;
|
||||
};
|
||||
|
||||
const matrix::Vector<float, 25>& vector() const {
|
||||
return *reinterpret_cast<matrix::Vector<float, 25>*>(const_cast<float*>(reinterpret_cast<const float*>(&quat_nominal)));
|
||||
const matrix::Vector<float, 26>& vector() const {
|
||||
return *reinterpret_cast<matrix::Vector<float, 26>*>(const_cast<float*>(reinterpret_cast<const float*>(&quat_nominal)));
|
||||
};
|
||||
|
||||
};
|
||||
static_assert(sizeof(matrix::Vector<float, 25>) == sizeof(StateSample), "state vector doesn't match StateSample size");
|
||||
static_assert(sizeof(matrix::Vector<float, 26>) == sizeof(StateSample), "state vector doesn't match StateSample size");
|
||||
|
||||
struct IdxDof { unsigned idx; unsigned dof; };
|
||||
namespace State {
|
||||
|
|
@ -52,7 +54,8 @@ namespace State {
|
|||
static constexpr IdxDof mag_B{18, 3};
|
||||
static constexpr IdxDof wind_vel{21, 2};
|
||||
static constexpr IdxDof flow_scale{23, 1};
|
||||
static constexpr uint8_t size{24};
|
||||
static constexpr IdxDof flow_exp{24, 1};
|
||||
static constexpr uint8_t size{25};
|
||||
};
|
||||
}
|
||||
#endif // !EKF_STATE_H
|
||||
|
|
|
|||
Loading…
Reference in New Issue