forked from Archive/PX4-Autopilot
Compare commits
3 Commits
main
...
pr-ekf2-te
| Author | SHA1 | Date |
|---|---|---|
bresch
|
be15d3d898 | |
|
bresch
|
35f1aa9e43 | |
|
bresch
|
28356d9120 |
|
|
@ -1,7 +1,7 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
|
||||
|
||||
float32[24] states # Internal filter states
|
||||
float32[25] states # Internal filter states
|
||||
uint8 n_states # Number of states effectively used
|
||||
|
||||
float32[23] covariances # Diagonal Elements of Covariance Matrix
|
||||
float32[24] covariances # Diagonal Elements of Covariance Matrix
|
||||
|
|
|
|||
|
|
@ -423,10 +423,8 @@ struct parameters {
|
|||
float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec)
|
||||
float range_noise{0.1f}; ///< observation noise for range finder measurements (m)
|
||||
float range_innov_gate{5.0f}; ///< range finder fusion innovation consistency gate size (STD)
|
||||
float rng_hgt_bias_nsd{0.13f}; ///< process noise for range height bias estimation (m/s/sqrt(Hz))
|
||||
float rng_sens_pitch{0.0f}; ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
|
||||
float range_noise_scaler{0.0f}; ///< scaling from range measurement to noise (m/m)
|
||||
const float vehicle_variance_scaler{0.0f}; ///< gain applied to vehicle height variance used in calculation of height above ground observation variance
|
||||
float max_hagl_for_range_aid{5.0f}; ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1)
|
||||
float max_vel_for_range_aid{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1)
|
||||
float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion
|
||||
|
|
|
|||
|
|
@ -99,6 +99,10 @@ void Ekf::initialiseCovariance()
|
|||
#if defined(CONFIG_EKF2_WIND)
|
||||
resetWindCov();
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
P(State::terrain_vpos.idx, State::terrain_vpos.idx) = sq(_params.rng_gnd_clearance);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER
|
||||
}
|
||||
|
||||
void Ekf::predictCovariance(const imuSample &imu_delayed)
|
||||
|
|
@ -207,6 +211,18 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
|
|||
}
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
if (_control_status.flags.opt_flow || _control_status.flags.rng_hgt) {
|
||||
// process noise due to errors in vehicle height estimate
|
||||
const float var_static = sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise);
|
||||
|
||||
// process noise due to terrain gradient
|
||||
const float var_dynamic = sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(1)));
|
||||
|
||||
P(State::terrain_vpos.idx, State::terrain_vpos.idx) += var_static + var_dynamic;
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
// covariance matrix is symmetrical, so copy upper half to lower half
|
||||
for (unsigned row = 0; row < State::size; row++) {
|
||||
for (unsigned column = 0 ; column < row; column++) {
|
||||
|
|
@ -327,7 +343,16 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
|
|||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
if (!_control_status.flags.rng_hgt && !_control_status.flags.opt_flow) {
|
||||
P.uncorrelateCovarianceSetVariance<State::terrain_vpos.dof>(State::terrain_vpos.idx, 0.f);
|
||||
|
||||
} else {
|
||||
constrainStateVar(State::terrain_vpos, 0.f, 1e4f);
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -72,6 +72,10 @@ void Ekf::reset()
|
|||
_state.wind_vel.setZero();
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER) || defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_state.terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER || CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_range_sensor.setPitchOffset(_params.rng_sens_pitch);
|
||||
_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
|
||||
|
|
|
|||
|
|
@ -98,7 +98,7 @@ public:
|
|||
uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; }
|
||||
|
||||
// get the estimated terrain vertical position relative to the NED origin
|
||||
float getTerrainVertPos() const { return _terrain_vpos; };
|
||||
float getTerrainVertPos() const { return _state.terrain_vpos; };
|
||||
|
||||
// get the number of times the vertical terrain position has been reset
|
||||
uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; };
|
||||
|
|
@ -837,10 +837,7 @@ private:
|
|||
void runTerrainEstimator(const imuSample &imu_delayed);
|
||||
void predictHagl(const imuSample &imu_delayed);
|
||||
|
||||
float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
|
||||
|
||||
void controlHaglFakeFusion();
|
||||
void terrainHandleVerticalPositionReset(float delta_z);
|
||||
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// update the terrain vertical position estimate using a height above ground measurement from the range finder
|
||||
|
|
|
|||
|
|
@ -207,13 +207,9 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
|
|||
#if defined(CONFIG_EKF2_GNSS)
|
||||
_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
terrainHandleVerticalPositionReset(delta_z);
|
||||
#endif
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER) || defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_state.terrain_vpos += delta_z;
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER || CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_hgt_fuse = _time_delayed_us;
|
||||
|
|
@ -247,6 +243,10 @@ void Ekf::constrainStates()
|
|||
#if defined(CONFIG_EKF2_WIND)
|
||||
_state.wind_vel = matrix::constrain(_state.wind_vel, -100.0f, 100.0f);
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER) || defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_state.terrain_vpos = matrix::constrain(_state.terrain_vpos, -1.e6f, 1.e6f);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER || CONFIG_EKF2_OPTICAL_FLOW
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
|
||||
|
|
@ -774,6 +774,7 @@ void Ekf::fuse(const VectorState &K, float innovation)
|
|||
#if defined(CONFIG_EKF2_WIND)
|
||||
_state.wind_vel -= K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) * innovation;
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
_state.terrain_vpos -= (K.slice<State::terrain_vpos.dof, 1>(State::terrain_vpos.idx, 0) * innovation)(0, 0);
|
||||
}
|
||||
|
||||
void Ekf::uncorrelateQuatFromOtherStates()
|
||||
|
|
|
|||
|
|
@ -130,7 +130,7 @@ bool Ekf::checkHaglYawResetReq() const
|
|||
// Check if height has increased sufficiently to be away from ground magnetic anomalies
|
||||
// and request a yaw reset if not already requested.
|
||||
static constexpr float mag_anomalies_max_hagl = 1.5f;
|
||||
const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
|
||||
const bool above_mag_anomalies = (getTerrainVertPos() - _state.pos(2)) > mag_anomalies_max_hagl;
|
||||
return above_mag_anomalies;
|
||||
}
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
|
|
|||
|
|
@ -79,7 +79,7 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
|
|||
|
||||
Vector2f innov_var;
|
||||
VectorState H;
|
||||
sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
|
||||
sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, R_LOS, FLT_EPSILON, &innov_var, &H);
|
||||
innov_var.copyTo(aid_src.innovation_variance);
|
||||
|
||||
// run the innovation consistency check and record result
|
||||
|
|
@ -98,7 +98,7 @@ void Ekf::fuseOptFlow()
|
|||
|
||||
Vector2f innov_var;
|
||||
VectorState H;
|
||||
sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
|
||||
sym::ComputeFlowXyInnovVarAndHx(state_vector, P, R_LOS, FLT_EPSILON, &innov_var, &H);
|
||||
innov_var.copyTo(_aid_src_optical_flow.innovation_variance);
|
||||
|
||||
if ((_aid_src_optical_flow.innovation_variance[0] < R_LOS)
|
||||
|
|
@ -129,7 +129,7 @@ void Ekf::fuseOptFlow()
|
|||
|
||||
} else if (index == 1) {
|
||||
// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
|
||||
sym::ComputeFlowYInnovVarAndH(state_vector, P, range, R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H);
|
||||
sym::ComputeFlowYInnovVarAndH(state_vector, P, R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H);
|
||||
|
||||
// recalculate the innovation using the updated state
|
||||
const Vector2f vel_body = predictFlowVelBody();
|
||||
|
|
@ -170,7 +170,7 @@ float Ekf::predictFlowRange()
|
|||
|
||||
// calculate the height above the ground of the optical flow camera. Since earth frame is NED
|
||||
// a positive offset in earth frame leads to a smaller height above the ground.
|
||||
const float height_above_gnd_est = math::max(_terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));
|
||||
const float height_above_gnd_est = math::max(_state.terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));
|
||||
|
||||
// calculate range from focal point to centre of image
|
||||
return height_above_gnd_est / _R_to_earth(2, 2); // absolute distance to the frame region in view
|
||||
|
|
|
|||
|
|
@ -68,7 +68,8 @@ State = Values(
|
|||
accel_bias = sf.V3(),
|
||||
mag_I = sf.V3(),
|
||||
mag_B = sf.V3(),
|
||||
wind_vel = sf.V2()
|
||||
wind_vel = sf.V2(),
|
||||
terrain_vpos = sf.V1()
|
||||
)
|
||||
|
||||
if args.disable_mag:
|
||||
|
|
@ -132,7 +133,8 @@ def predict_covariance(
|
|||
accel_bias = sf.V3.symbolic("delta_a_b"),
|
||||
mag_I = sf.V3.symbolic("mag_I"),
|
||||
mag_B = sf.V3.symbolic("mag_B"),
|
||||
wind_vel = sf.V2.symbolic("wind_vel")
|
||||
wind_vel = sf.V2.symbolic("wind_vel"),
|
||||
terrain_vpos = sf.V1.symbolic("terrain_vpos")
|
||||
)
|
||||
|
||||
if args.disable_mag:
|
||||
|
|
@ -473,18 +475,20 @@ def compute_mag_declination_pred_innov_var_and_h(
|
|||
|
||||
return (meas_pred, innov_var, H.T)
|
||||
|
||||
def predict_opt_flow(state, distance, epsilon):
|
||||
def predict_opt_flow(state, epsilon):
|
||||
R_to_body = state["quat_nominal"].inverse()
|
||||
|
||||
# Calculate earth relative velocity in a non-rotating sensor frame
|
||||
rel_vel_sensor = R_to_body * state["vel"]
|
||||
hagl = state["terrain_vpos"][0] - state["pos"][2]
|
||||
dist = hagl / state["quat_nominal"].to_rotation_matrix()[2, 2]
|
||||
|
||||
# Divide by range to get predicted angular LOS rates relative to X and Y
|
||||
# axes. Note these are rates in a non-rotating sensor frame
|
||||
flow_pred = sf.V2()
|
||||
flow_pred[0] = rel_vel_sensor[1] / distance
|
||||
flow_pred[1] = -rel_vel_sensor[0] / distance
|
||||
flow_pred = add_epsilon_sign(flow_pred, distance, epsilon)
|
||||
flow_pred[0] = rel_vel_sensor[1] / dist
|
||||
flow_pred[1] = -rel_vel_sensor[0] / dist
|
||||
flow_pred = add_epsilon_sign(flow_pred, hagl, epsilon)
|
||||
|
||||
return flow_pred
|
||||
|
||||
|
|
@ -492,12 +496,11 @@ def predict_opt_flow(state, distance, epsilon):
|
|||
def compute_flow_xy_innov_var_and_hx(
|
||||
state: VState,
|
||||
P: MTangent,
|
||||
distance: sf.Scalar,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.V2, VTangent):
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_opt_flow(state, distance, epsilon);
|
||||
meas_pred = predict_opt_flow(state, epsilon);
|
||||
|
||||
innov_var = sf.V2()
|
||||
Hx = sf.V1(meas_pred[0]).jacobian(state)
|
||||
|
|
@ -510,12 +513,11 @@ def compute_flow_xy_innov_var_and_hx(
|
|||
def compute_flow_y_innov_var_and_h(
|
||||
state: VState,
|
||||
P: MTangent,
|
||||
distance: sf.Scalar,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VTangent):
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_opt_flow(state, distance, epsilon);
|
||||
meas_pred = predict_opt_flow(state, epsilon);
|
||||
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state)
|
||||
innov_var = (Hy * P * Hy.T + R)[0,0]
|
||||
|
|
|
|||
|
|
@ -16,21 +16,21 @@ namespace sym {
|
|||
* Symbolic function: compute_airspeed_h_and_k
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* innov_var: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* H: Matrix23_1
|
||||
* K: Matrix23_1
|
||||
* H: Matrix24_1
|
||||
* K: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeAirspeedHAndK(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: 246
|
||||
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
|
||||
|
||||
// Input arrays
|
||||
|
||||
|
|
@ -47,7 +47,7 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
|||
|
||||
// Output terms (2)
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
@ -59,7 +59,7 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
|||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _k = (*K);
|
||||
matrix::Matrix<Scalar, 24, 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);
|
||||
|
|
@ -107,6 +107,8 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
|||
P(21, 4) * _tmp4 + P(21, 5) * _tmp5);
|
||||
_k(22, 0) = _tmp6 * (-P(22, 21) * _tmp3 - P(22, 22) * _tmp4 + P(22, 3) * _tmp3 +
|
||||
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);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
|
|
|||
|
|
@ -16,8 +16,8 @@ namespace sym {
|
|||
* Symbolic function: compute_airspeed_innov_and_innov_var
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* airspeed: Scalar
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
|
|
@ -27,8 +27,8 @@ namespace sym {
|
|||
* innov_var: Scalar
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar airspeed,
|
||||
void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& 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: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* rho: Scalar
|
||||
* cd: Scalar
|
||||
* cm: Scalar
|
||||
|
|
@ -26,14 +26,14 @@ namespace sym {
|
|||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* K: Matrix23_1
|
||||
* K: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar rho,
|
||||
void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar rho,
|
||||
const Scalar cd, const Scalar cm, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const K = nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
|
||||
// Total ops: 348
|
||||
|
||||
// Input arrays
|
||||
|
|
@ -153,7 +153,7 @@ void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
|||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _k = (*K);
|
||||
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
|
||||
|
||||
_k.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,8 +16,8 @@ namespace sym {
|
|||
* Symbolic function: compute_drag_y_innov_var_and_k
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* rho: Scalar
|
||||
* cd: Scalar
|
||||
* cm: Scalar
|
||||
|
|
@ -26,14 +26,14 @@ namespace sym {
|
|||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* K: Matrix23_1
|
||||
* K: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeDragYInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar rho,
|
||||
void ComputeDragYInnovVarAndK(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar rho,
|
||||
const Scalar cd, const Scalar cm, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const K = nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
|
||||
// Total ops: 348
|
||||
|
||||
// Input arrays
|
||||
|
|
@ -155,7 +155,7 @@ void ComputeDragYInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
|||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _k = (*K);
|
||||
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
|
||||
|
||||
_k.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,105 +16,146 @@ namespace sym {
|
|||
* Symbolic function: compute_flow_xy_innov_var_and_hx
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* distance: Scalar
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Matrix21
|
||||
* H: Matrix23_1
|
||||
* H: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar distance,
|
||||
const Scalar R, const Scalar epsilon,
|
||||
void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, 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
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 383
|
||||
|
||||
// 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 =
|
||||
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 _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);
|
||||
// Intermediate terms (56)
|
||||
const Scalar _tmp0 = 2 * state(0, 0);
|
||||
const Scalar _tmp1 = _tmp0 * state(3, 0);
|
||||
const Scalar _tmp2 = -_tmp1;
|
||||
const Scalar _tmp3 = 2 * state(2, 0);
|
||||
const Scalar _tmp4 = _tmp3 * state(1, 0);
|
||||
const Scalar _tmp5 = _tmp2 + _tmp4;
|
||||
const Scalar _tmp6 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp7 = -2 * _tmp6;
|
||||
const Scalar _tmp8 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp9 = 1 - 2 * _tmp8;
|
||||
const Scalar _tmp10 = _tmp7 + _tmp9;
|
||||
const Scalar _tmp11 = state(24, 0) - state(9, 0);
|
||||
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 = _tmp10 * _tmp13;
|
||||
const Scalar _tmp15 = _tmp14 * _tmp5;
|
||||
const Scalar _tmp16 = _tmp0 * state(2, 0);
|
||||
const Scalar _tmp17 = 2 * state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp18 = _tmp3 * state(3, 0);
|
||||
const Scalar _tmp19 = _tmp0 * state(1, 0);
|
||||
const Scalar _tmp20 = -_tmp19;
|
||||
const Scalar _tmp21 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp22 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp23 = -_tmp6;
|
||||
const Scalar _tmp24 = _tmp22 + _tmp23;
|
||||
const Scalar _tmp25 = -_tmp18;
|
||||
const Scalar _tmp26 = _tmp20 + _tmp25;
|
||||
const Scalar _tmp27 = -2 * _tmp21;
|
||||
const Scalar _tmp28 = _tmp27 + _tmp7 + 1;
|
||||
const Scalar _tmp29 = _tmp18 + _tmp19;
|
||||
const Scalar _tmp30 = _tmp29 * state(6, 0) + _tmp5 * state(4, 0);
|
||||
const Scalar _tmp31 = _tmp28 * state(5, 0) + _tmp30;
|
||||
const Scalar _tmp32 = _tmp13 * _tmp31;
|
||||
const Scalar _tmp33 =
|
||||
_tmp14 * (state(4, 0) * (_tmp16 + _tmp17) + state(5, 0) * (_tmp18 + _tmp20) +
|
||||
state(6, 0) * (_tmp21 + _tmp24 - _tmp8)) +
|
||||
_tmp26 * _tmp32;
|
||||
const Scalar _tmp34 = -_tmp16;
|
||||
const Scalar _tmp35 = _tmp17 + _tmp34;
|
||||
const Scalar _tmp36 = _tmp32 * _tmp35;
|
||||
const Scalar _tmp37 = _tmp14 * _tmp28;
|
||||
const Scalar _tmp38 = _tmp10 / std::pow(_tmp12, Scalar(2));
|
||||
const Scalar _tmp39 = _tmp31 * _tmp38;
|
||||
const Scalar _tmp40 = -_tmp17;
|
||||
const Scalar _tmp41 = -_tmp22;
|
||||
const Scalar _tmp42 = _tmp14 * (state(4, 0) * (_tmp21 + _tmp23 + _tmp41 + _tmp8) +
|
||||
state(5, 0) * (_tmp2 - _tmp4) + state(6, 0) * (_tmp16 + _tmp40));
|
||||
const Scalar _tmp43 = _tmp14 * _tmp29;
|
||||
const Scalar _tmp44 = _tmp14 * _tmp35;
|
||||
const Scalar _tmp45 = _tmp1 + _tmp4;
|
||||
const Scalar _tmp46 = _tmp14 * _tmp45;
|
||||
const Scalar _tmp47 = _tmp27 + _tmp9;
|
||||
const Scalar _tmp48 = _tmp14 * _tmp47;
|
||||
const Scalar _tmp49 = -_tmp21 + _tmp8;
|
||||
const Scalar _tmp50 = _tmp35 * state(6, 0) + _tmp45 * state(5, 0) + _tmp47 * state(4, 0);
|
||||
const Scalar _tmp51 = _tmp13 * _tmp50;
|
||||
const Scalar _tmp52 =
|
||||
-_tmp14 * (state(4, 0) * (_tmp34 + _tmp40) + state(5, 0) * (_tmp19 + _tmp25) +
|
||||
state(6, 0) * (_tmp41 + _tmp49 + _tmp6)) -
|
||||
_tmp35 * _tmp51;
|
||||
const Scalar _tmp53 = _tmp38 * _tmp50;
|
||||
const Scalar _tmp54 = _tmp14 * (_tmp30 + state(5, 0) * (_tmp24 + _tmp49));
|
||||
const Scalar _tmp55 = _tmp26 * _tmp51;
|
||||
|
||||
// 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);
|
||||
_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);
|
||||
_innov_var(0, 0) =
|
||||
R +
|
||||
_tmp15 * (P(0, 3) * _tmp33 + P(1, 3) * _tmp36 + P(2, 3) * _tmp42 - P(23, 3) * _tmp39 +
|
||||
P(3, 3) * _tmp15 + P(4, 3) * _tmp37 + P(5, 3) * _tmp43 + P(8, 3) * _tmp39) +
|
||||
_tmp33 * (P(0, 0) * _tmp33 + P(1, 0) * _tmp36 + P(2, 0) * _tmp42 - P(23, 0) * _tmp39 +
|
||||
P(3, 0) * _tmp15 + P(4, 0) * _tmp37 + P(5, 0) * _tmp43 + P(8, 0) * _tmp39) +
|
||||
_tmp36 * (P(0, 1) * _tmp33 + P(1, 1) * _tmp36 + P(2, 1) * _tmp42 - P(23, 1) * _tmp39 +
|
||||
P(3, 1) * _tmp15 + P(4, 1) * _tmp37 + P(5, 1) * _tmp43 + P(8, 1) * _tmp39) +
|
||||
_tmp37 * (P(0, 4) * _tmp33 + P(1, 4) * _tmp36 + P(2, 4) * _tmp42 - P(23, 4) * _tmp39 +
|
||||
P(3, 4) * _tmp15 + P(4, 4) * _tmp37 + P(5, 4) * _tmp43 + P(8, 4) * _tmp39) -
|
||||
_tmp39 * (P(0, 23) * _tmp33 + P(1, 23) * _tmp36 + P(2, 23) * _tmp42 - P(23, 23) * _tmp39 +
|
||||
P(3, 23) * _tmp15 + P(4, 23) * _tmp37 + P(5, 23) * _tmp43 + P(8, 23) * _tmp39) +
|
||||
_tmp39 * (P(0, 8) * _tmp33 + P(1, 8) * _tmp36 + P(2, 8) * _tmp42 - P(23, 8) * _tmp39 +
|
||||
P(3, 8) * _tmp15 + P(4, 8) * _tmp37 + P(5, 8) * _tmp43 + P(8, 8) * _tmp39) +
|
||||
_tmp42 * (P(0, 2) * _tmp33 + P(1, 2) * _tmp36 + P(2, 2) * _tmp42 - P(23, 2) * _tmp39 +
|
||||
P(3, 2) * _tmp15 + P(4, 2) * _tmp37 + P(5, 2) * _tmp43 + P(8, 2) * _tmp39) +
|
||||
_tmp43 * (P(0, 5) * _tmp33 + P(1, 5) * _tmp36 + P(2, 5) * _tmp42 - P(23, 5) * _tmp39 +
|
||||
P(3, 5) * _tmp15 + P(4, 5) * _tmp37 + P(5, 5) * _tmp43 + P(8, 5) * _tmp39);
|
||||
_innov_var(1, 0) =
|
||||
R -
|
||||
_tmp44 * (-P(0, 5) * _tmp55 + P(1, 5) * _tmp52 - P(2, 5) * _tmp54 + P(23, 5) * _tmp53 -
|
||||
P(3, 5) * _tmp48 - P(4, 5) * _tmp46 - P(5, 5) * _tmp44 - P(8, 5) * _tmp53) -
|
||||
_tmp46 * (-P(0, 4) * _tmp55 + P(1, 4) * _tmp52 - P(2, 4) * _tmp54 + P(23, 4) * _tmp53 -
|
||||
P(3, 4) * _tmp48 - P(4, 4) * _tmp46 - P(5, 4) * _tmp44 - P(8, 4) * _tmp53) -
|
||||
_tmp48 * (-P(0, 3) * _tmp55 + P(1, 3) * _tmp52 - P(2, 3) * _tmp54 + P(23, 3) * _tmp53 -
|
||||
P(3, 3) * _tmp48 - P(4, 3) * _tmp46 - P(5, 3) * _tmp44 - P(8, 3) * _tmp53) +
|
||||
_tmp52 * (-P(0, 1) * _tmp55 + P(1, 1) * _tmp52 - P(2, 1) * _tmp54 + P(23, 1) * _tmp53 -
|
||||
P(3, 1) * _tmp48 - P(4, 1) * _tmp46 - P(5, 1) * _tmp44 - P(8, 1) * _tmp53) +
|
||||
_tmp53 * (-P(0, 23) * _tmp55 + P(1, 23) * _tmp52 - P(2, 23) * _tmp54 + P(23, 23) * _tmp53 -
|
||||
P(3, 23) * _tmp48 - P(4, 23) * _tmp46 - P(5, 23) * _tmp44 - P(8, 23) * _tmp53) -
|
||||
_tmp53 * (-P(0, 8) * _tmp55 + P(1, 8) * _tmp52 - P(2, 8) * _tmp54 + P(23, 8) * _tmp53 -
|
||||
P(3, 8) * _tmp48 - P(4, 8) * _tmp46 - P(5, 8) * _tmp44 - P(8, 8) * _tmp53) -
|
||||
_tmp54 * (-P(0, 2) * _tmp55 + P(1, 2) * _tmp52 - P(2, 2) * _tmp54 + P(23, 2) * _tmp53 -
|
||||
P(3, 2) * _tmp48 - P(4, 2) * _tmp46 - P(5, 2) * _tmp44 - P(8, 2) * _tmp53) -
|
||||
_tmp55 * (-P(0, 0) * _tmp55 + P(1, 0) * _tmp52 - P(2, 0) * _tmp54 + P(23, 0) * _tmp53 -
|
||||
P(3, 0) * _tmp48 - P(4, 0) * _tmp46 - P(5, 0) * _tmp44 - P(8, 0) * _tmp53);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_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) = _tmp33;
|
||||
_h(1, 0) = _tmp36;
|
||||
_h(2, 0) = _tmp42;
|
||||
_h(3, 0) = _tmp15;
|
||||
_h(4, 0) = _tmp37;
|
||||
_h(5, 0) = _tmp43;
|
||||
_h(8, 0) = _tmp39;
|
||||
_h(23, 0) = -_tmp39;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
|
|
|||
|
|
@ -16,79 +16,100 @@ namespace sym {
|
|||
* Symbolic function: compute_flow_y_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* distance: Scalar
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix23_1
|
||||
* H: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar distance,
|
||||
const Scalar R, const Scalar epsilon,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
// Total ops: 116
|
||||
void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 212
|
||||
|
||||
// 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 =
|
||||
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 _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);
|
||||
// Intermediate terms (32)
|
||||
const Scalar _tmp0 = 2 * state(2, 0);
|
||||
const Scalar _tmp1 = -_tmp0 * state(0, 0);
|
||||
const Scalar _tmp2 = 2 * state(1, 0);
|
||||
const Scalar _tmp3 = _tmp2 * state(3, 0);
|
||||
const Scalar _tmp4 = _tmp1 + _tmp3;
|
||||
const Scalar _tmp5 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp6 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp7 = 1 - 2 * _tmp6;
|
||||
const Scalar _tmp8 = -2 * _tmp5 + _tmp7;
|
||||
const Scalar _tmp9 = state(24, 0) - state(9, 0);
|
||||
const Scalar _tmp10 =
|
||||
_tmp9 + epsilon * (2 * math::min<Scalar>(0, (((_tmp9) > 0) - ((_tmp9) < 0))) + 1);
|
||||
const Scalar _tmp11 = Scalar(1.0) / (_tmp10);
|
||||
const Scalar _tmp12 = _tmp11 * _tmp8;
|
||||
const Scalar _tmp13 = _tmp12 * _tmp4;
|
||||
const Scalar _tmp14 = 2 * state(0, 0) * state(3, 0);
|
||||
const Scalar _tmp15 = _tmp0 * state(1, 0);
|
||||
const Scalar _tmp16 = _tmp14 + _tmp15;
|
||||
const Scalar _tmp17 = _tmp12 * _tmp16;
|
||||
const Scalar _tmp18 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp19 = -2 * _tmp18 + _tmp7;
|
||||
const Scalar _tmp20 = _tmp12 * _tmp19;
|
||||
const Scalar _tmp21 = _tmp0 * state(3, 0);
|
||||
const Scalar _tmp22 = -_tmp21;
|
||||
const Scalar _tmp23 = _tmp2 * state(0, 0);
|
||||
const Scalar _tmp24 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp25 = -_tmp18 + _tmp6;
|
||||
const Scalar _tmp26 = _tmp16 * state(5, 0) + _tmp19 * state(4, 0) + _tmp4 * state(6, 0);
|
||||
const Scalar _tmp27 = _tmp11 * _tmp26;
|
||||
const Scalar _tmp28 = -_tmp12 * (state(4, 0) * (_tmp1 - _tmp3) + state(5, 0) * (_tmp22 + _tmp23) +
|
||||
state(6, 0) * (-_tmp24 + _tmp25 + _tmp5)) -
|
||||
_tmp27 * _tmp4;
|
||||
const Scalar _tmp29 = _tmp26 * _tmp8 / std::pow(_tmp10, Scalar(2));
|
||||
const Scalar _tmp30 =
|
||||
_tmp12 * (state(4, 0) * (-_tmp14 + _tmp15) + state(5, 0) * (_tmp24 + _tmp25 - _tmp5) +
|
||||
state(6, 0) * (_tmp21 + _tmp23));
|
||||
const Scalar _tmp31 = _tmp27 * (_tmp22 - _tmp23);
|
||||
|
||||
// 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);
|
||||
_innov_var =
|
||||
R -
|
||||
_tmp13 * (-P(0, 5) * _tmp31 + P(1, 5) * _tmp28 - P(2, 5) * _tmp30 + P(23, 5) * _tmp29 -
|
||||
P(3, 5) * _tmp20 - P(4, 5) * _tmp17 - P(5, 5) * _tmp13 - P(8, 5) * _tmp29) -
|
||||
_tmp17 * (-P(0, 4) * _tmp31 + P(1, 4) * _tmp28 - P(2, 4) * _tmp30 + P(23, 4) * _tmp29 -
|
||||
P(3, 4) * _tmp20 - P(4, 4) * _tmp17 - P(5, 4) * _tmp13 - P(8, 4) * _tmp29) -
|
||||
_tmp20 * (-P(0, 3) * _tmp31 + P(1, 3) * _tmp28 - P(2, 3) * _tmp30 + P(23, 3) * _tmp29 -
|
||||
P(3, 3) * _tmp20 - P(4, 3) * _tmp17 - P(5, 3) * _tmp13 - P(8, 3) * _tmp29) +
|
||||
_tmp28 * (-P(0, 1) * _tmp31 + P(1, 1) * _tmp28 - P(2, 1) * _tmp30 + P(23, 1) * _tmp29 -
|
||||
P(3, 1) * _tmp20 - P(4, 1) * _tmp17 - P(5, 1) * _tmp13 - P(8, 1) * _tmp29) +
|
||||
_tmp29 * (-P(0, 23) * _tmp31 + P(1, 23) * _tmp28 - P(2, 23) * _tmp30 + P(23, 23) * _tmp29 -
|
||||
P(3, 23) * _tmp20 - P(4, 23) * _tmp17 - P(5, 23) * _tmp13 - P(8, 23) * _tmp29) -
|
||||
_tmp29 * (-P(0, 8) * _tmp31 + P(1, 8) * _tmp28 - P(2, 8) * _tmp30 + P(23, 8) * _tmp29 -
|
||||
P(3, 8) * _tmp20 - P(4, 8) * _tmp17 - P(5, 8) * _tmp13 - P(8, 8) * _tmp29) -
|
||||
_tmp30 * (-P(0, 2) * _tmp31 + P(1, 2) * _tmp28 - P(2, 2) * _tmp30 + P(23, 2) * _tmp29 -
|
||||
P(3, 2) * _tmp20 - P(4, 2) * _tmp17 - P(5, 2) * _tmp13 - P(8, 2) * _tmp29) -
|
||||
_tmp31 * (-P(0, 0) * _tmp31 + P(1, 0) * _tmp28 - P(2, 0) * _tmp30 + P(23, 0) * _tmp29 -
|
||||
P(3, 0) * _tmp20 - P(4, 0) * _tmp17 - P(5, 0) * _tmp13 - P(8, 0) * _tmp29);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(1, 0) = -_tmp13;
|
||||
_h(2, 0) = -_tmp16;
|
||||
_h(3, 0) = -_tmp3;
|
||||
_h(0, 0) = -_tmp31;
|
||||
_h(1, 0) = _tmp28;
|
||||
_h(2, 0) = -_tmp30;
|
||||
_h(3, 0) = -_tmp20;
|
||||
_h(4, 0) = -_tmp17;
|
||||
_h(5, 0) = -_tmp18;
|
||||
_h(5, 0) = -_tmp13;
|
||||
_h(8, 0) = -_tmp29;
|
||||
_h(23, 0) = _tmp29;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
|
|
|||
|
|
@ -16,8 +16,8 @@ namespace sym {
|
|||
* Symbolic function: compute_gnss_yaw_pred_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* antenna_yaw_offset: Scalar
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
|
|
@ -25,15 +25,15 @@ namespace sym {
|
|||
* Outputs:
|
||||
* meas_pred: Scalar
|
||||
* innov_var: Scalar
|
||||
* H: Matrix23_1
|
||||
* H: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P,
|
||||
void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& 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, 23, 1>* const H = nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 95
|
||||
|
||||
// Input arrays
|
||||
|
|
@ -88,7 +88,7 @@ void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
|||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,8 +16,8 @@ namespace sym {
|
|||
* Symbolic function: compute_gravity_innov_var_and_k_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* meas: Matrix31
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
|
|
@ -25,21 +25,21 @@ namespace sym {
|
|||
* Outputs:
|
||||
* innov: Matrix31
|
||||
* innov_var: Matrix31
|
||||
* Kx: Matrix23_1
|
||||
* Ky: Matrix23_1
|
||||
* Kz: Matrix23_1
|
||||
* Kx: Matrix24_1
|
||||
* Ky: Matrix24_1
|
||||
* Kz: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P,
|
||||
void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P,
|
||||
const matrix::Matrix<Scalar, 3, 1>& meas, const Scalar R,
|
||||
const Scalar epsilon,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov = nullptr,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Kx = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Ky = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Kz = nullptr) {
|
||||
// Total ops: 361
|
||||
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
|
||||
|
||||
// Input arrays
|
||||
|
||||
|
|
@ -103,7 +103,7 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
|||
}
|
||||
|
||||
if (Kx != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _kx = (*Kx);
|
||||
matrix::Matrix<Scalar, 24, 1>& _kx = (*Kx);
|
||||
|
||||
_kx(0, 0) = _tmp28 * (P(0, 1) * _tmp13 + P(0, 2) * _tmp14);
|
||||
_kx(1, 0) = _tmp28 * (P(1, 2) * _tmp14 + _tmp16);
|
||||
|
|
@ -128,10 +128,11 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
|||
_kx(20, 0) = _tmp28 * (P(20, 1) * _tmp13 + P(20, 2) * _tmp14);
|
||||
_kx(21, 0) = _tmp28 * (P(21, 1) * _tmp13 + P(21, 2) * _tmp14);
|
||||
_kx(22, 0) = _tmp28 * (P(22, 1) * _tmp13 + P(22, 2) * _tmp14);
|
||||
_kx(23, 0) = _tmp28 * (P(23, 1) * _tmp13 + P(23, 2) * _tmp14);
|
||||
}
|
||||
|
||||
if (Ky != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _ky = (*Ky);
|
||||
matrix::Matrix<Scalar, 24, 1>& _ky = (*Ky);
|
||||
|
||||
_ky(0, 0) = _tmp29 * (P(0, 2) * _tmp19 + _tmp21);
|
||||
_ky(1, 0) = _tmp29 * (P(1, 0) * _tmp18 + P(1, 2) * _tmp19);
|
||||
|
|
@ -156,10 +157,11 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
|||
_ky(20, 0) = _tmp29 * (P(20, 0) * _tmp18 + P(20, 2) * _tmp19);
|
||||
_ky(21, 0) = _tmp29 * (P(21, 0) * _tmp18 + P(21, 2) * _tmp19);
|
||||
_ky(22, 0) = _tmp29 * (P(22, 0) * _tmp18 + P(22, 2) * _tmp19);
|
||||
_ky(23, 0) = _tmp29 * (P(23, 0) * _tmp18 + P(23, 2) * _tmp19);
|
||||
}
|
||||
|
||||
if (Kz != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _kz = (*Kz);
|
||||
matrix::Matrix<Scalar, 24, 1>& _kz = (*Kz);
|
||||
|
||||
_kz(0, 0) = _tmp30 * (P(0, 1) * _tmp24 + _tmp26);
|
||||
_kz(1, 0) = _tmp30 * (P(1, 0) * _tmp23 + _tmp25);
|
||||
|
|
@ -184,6 +186,7 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
|||
_kz(20, 0) = _tmp30 * (P(20, 0) * _tmp23 + P(20, 1) * _tmp24);
|
||||
_kz(21, 0) = _tmp30 * (P(21, 0) * _tmp23 + P(21, 1) * _tmp24);
|
||||
_kz(22, 0) = _tmp30 * (P(22, 0) * _tmp23 + P(22, 1) * _tmp24);
|
||||
_kz(23, 0) = _tmp30 * (P(23, 0) * _tmp23 + P(23, 1) * _tmp24);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
|
|
|||
|
|
@ -16,22 +16,22 @@ namespace sym {
|
|||
* Symbolic function: compute_mag_declination_pred_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* pred: Scalar
|
||||
* innov_var: Scalar
|
||||
* H: Matrix23_1
|
||||
* H: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const pred = nullptr,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 22
|
||||
|
||||
// Input arrays
|
||||
|
|
@ -59,7 +59,7 @@ void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>&
|
|||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,8 +16,8 @@ namespace sym {
|
|||
* Symbolic function: compute_mag_innov_innov_var_and_hx
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* meas: Matrix31
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
|
|
@ -25,16 +25,16 @@ namespace sym {
|
|||
* Outputs:
|
||||
* innov: Matrix31
|
||||
* innov_var: Matrix31
|
||||
* Hx: Matrix23_1
|
||||
* Hx: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P,
|
||||
void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P,
|
||||
const matrix::Matrix<Scalar, 3, 1>& meas, const Scalar R,
|
||||
const Scalar epsilon,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov = nullptr,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Hx = nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>* const Hx = nullptr) {
|
||||
// Total ops: 314
|
||||
|
||||
// Unused inputs
|
||||
|
|
@ -146,7 +146,7 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
|||
}
|
||||
|
||||
if (Hx != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _hx = (*Hx);
|
||||
matrix::Matrix<Scalar, 24, 1>& _hx = (*Hx);
|
||||
|
||||
_hx.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,20 +16,20 @@ namespace sym {
|
|||
* Symbolic function: compute_mag_y_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix23_1
|
||||
* H: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 110
|
||||
|
||||
// Unused inputs
|
||||
|
|
@ -78,7 +78,7 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
|||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,20 +16,20 @@ namespace sym {
|
|||
* Symbolic function: compute_mag_z_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix23_1
|
||||
* H: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 110
|
||||
|
||||
// Unused inputs
|
||||
|
|
@ -78,7 +78,7 @@ void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
|||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,21 +16,21 @@ namespace sym {
|
|||
* Symbolic function: compute_sideslip_h_and_k
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* innov_var: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* H: Matrix23_1
|
||||
* K: Matrix23_1
|
||||
* H: Matrix24_1
|
||||
* K: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
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
|
||||
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
|
||||
|
||||
// Input arrays
|
||||
|
||||
|
|
@ -42,20 +42,20 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 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(2, 0);
|
||||
const Scalar _tmp8 = _tmp7 * state(1, 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 = _tmp7 * state(0, 0);
|
||||
const Scalar _tmp11 = _tmp5 * state(2, 0);
|
||||
const Scalar _tmp12 = -_tmp11;
|
||||
const Scalar _tmp13 = 2 * state(1, 0) * state(3, 0);
|
||||
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 = _tmp7 * state(3, 0);
|
||||
const Scalar _tmp19 = _tmp5 * state(1, 0);
|
||||
const Scalar _tmp18 = 2 * state(2, 0) * state(3, 0);
|
||||
const Scalar _tmp19 = _tmp7 * state(0, 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, 24, 1>& state,
|
|||
|
||||
// Output terms (2)
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
@ -105,7 +105,7 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
|||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _k = (*K);
|
||||
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
|
||||
|
||||
_k(0, 0) =
|
||||
_tmp45 * (P(0, 0) * _tmp24 - P(0, 1) * _tmp34 + P(0, 2) * _tmp35 + P(0, 21) * _tmp43 +
|
||||
|
|
@ -176,6 +176,9 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
|||
_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);
|
||||
_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);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
|
|
|||
|
|
@ -16,8 +16,8 @@ namespace sym {
|
|||
* Symbolic function: compute_sideslip_innov_and_innov_var
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
|
|
@ -26,8 +26,8 @@ namespace sym {
|
|||
* innov_var: Scalar
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov = nullptr,
|
||||
Scalar* const innov_var = nullptr) {
|
||||
// Total ops: 235
|
||||
|
|
|
|||
|
|
@ -16,20 +16,20 @@ namespace sym {
|
|||
* Symbolic function: compute_yaw_312_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix23_1
|
||||
* H: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeYaw312InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
void ComputeYaw312InnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 53
|
||||
|
||||
// Input arrays
|
||||
|
|
@ -63,7 +63,7 @@ void ComputeYaw312InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
|||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,20 +16,20 @@ namespace sym {
|
|||
* Symbolic function: compute_yaw_312_innov_var_and_h_alternate
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix23_1
|
||||
* H: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 57
|
||||
|
||||
// Input arrays
|
||||
|
|
@ -63,7 +63,7 @@ void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& sta
|
|||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,20 +16,20 @@ namespace sym {
|
|||
* Symbolic function: compute_yaw_321_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix23_1
|
||||
* H: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeYaw321InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
void ComputeYaw321InnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 53
|
||||
|
||||
// Input arrays
|
||||
|
|
@ -63,7 +63,7 @@ void ComputeYaw321InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
|||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,20 +16,20 @@ namespace sym {
|
|||
* Symbolic function: compute_yaw_321_innov_var_and_h_alternate
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix23_1
|
||||
* H: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 57
|
||||
|
||||
// Input arrays
|
||||
|
|
@ -63,7 +63,7 @@ void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& sta
|
|||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
|
|
|
|||
|
|
@ -16,8 +16,8 @@ namespace sym {
|
|||
* Symbolic function: predict_covariance
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* accel: Matrix31
|
||||
* accel_var: Matrix31
|
||||
* gyro: Matrix31
|
||||
|
|
@ -25,16 +25,16 @@ namespace sym {
|
|||
* dt: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* res: Matrix23_23
|
||||
* res: Matrix24_24
|
||||
*/
|
||||
template <typename Scalar>
|
||||
matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P,
|
||||
matrix::Matrix<Scalar, 24, 24> PredictCovariance(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P,
|
||||
const matrix::Matrix<Scalar, 3, 1>& accel,
|
||||
const matrix::Matrix<Scalar, 3, 1>& accel_var,
|
||||
const matrix::Matrix<Scalar, 3, 1>& gyro,
|
||||
const Scalar gyro_var, const Scalar dt) {
|
||||
// Total ops: 1793
|
||||
// Total ops: 1853
|
||||
|
||||
// Input arrays
|
||||
|
||||
|
|
@ -75,19 +75,19 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
const Scalar _tmp33 = _tmp32 * state(2, 0);
|
||||
const Scalar _tmp34 = _tmp31 + _tmp33;
|
||||
const Scalar _tmp35 = accel(0, 0) - state(13, 0);
|
||||
const Scalar _tmp36 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp36 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp37 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp38 = -_tmp37;
|
||||
const Scalar _tmp39 = _tmp36 + _tmp38;
|
||||
const Scalar _tmp40 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp41 = -_tmp40;
|
||||
const Scalar _tmp42 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp42 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp43 = _tmp41 + _tmp42;
|
||||
const Scalar _tmp44 = accel(1, 0) - state(14, 0);
|
||||
const Scalar _tmp45 = dt * (_tmp34 * _tmp35 + _tmp44 * (_tmp39 + _tmp43));
|
||||
const Scalar _tmp46 = P(0, 12) + P(1, 12) * _tmp5 + P(2, 12) * _tmp2 - P(9, 12) * dt;
|
||||
const Scalar _tmp47 = -2 * _tmp42;
|
||||
const Scalar _tmp48 = -2 * _tmp36;
|
||||
const Scalar _tmp47 = -2 * _tmp36;
|
||||
const Scalar _tmp48 = -2 * _tmp42;
|
||||
const Scalar _tmp49 = _tmp47 + _tmp48 + 1;
|
||||
const Scalar _tmp50 = _tmp49 * dt;
|
||||
const Scalar _tmp51 = _tmp29 * state(2, 0);
|
||||
|
|
@ -137,8 +137,9 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
const Scalar _tmp88 = _tmp29 * state(1, 0);
|
||||
const Scalar _tmp89 = -_tmp88;
|
||||
const Scalar _tmp90 = _tmp87 + _tmp89;
|
||||
const Scalar _tmp91 = dt * (_tmp44 * _tmp90 + _tmp59 * (_tmp43 + _tmp58));
|
||||
const Scalar _tmp92 = dt * (_tmp35 * (_tmp39 + _tmp56) + _tmp44 * (_tmp31 + _tmp62));
|
||||
const Scalar _tmp91 = dt * (_tmp44 * _tmp90 + _tmp59 * (_tmp36 + _tmp37 + _tmp41 + _tmp55));
|
||||
const Scalar _tmp92 =
|
||||
dt * (_tmp35 * (_tmp38 + _tmp40 + _tmp42 + _tmp57) + _tmp44 * (_tmp31 + _tmp62));
|
||||
const Scalar _tmp93 = 1 - 2 * _tmp37;
|
||||
const Scalar _tmp94 = _tmp47 + _tmp93;
|
||||
const Scalar _tmp95 = _tmp94 * dt;
|
||||
|
|
@ -152,52 +153,51 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
const Scalar _tmp103 = _tmp74 * dt;
|
||||
const Scalar _tmp104 = P(0, 4) * _tmp22 + P(1, 4) * _tmp23 - P(11, 4) * dt + P(2, 4);
|
||||
const Scalar _tmp105 = _tmp81 * _tmp97;
|
||||
const Scalar _tmp106 = _tmp84 * _tmp90;
|
||||
const Scalar _tmp107 = P(0, 4) * _tmp63 + P(1, 4) * _tmp60 - P(12, 4) * _tmp50 -
|
||||
const Scalar _tmp106 = _tmp83 * _tmp94;
|
||||
const Scalar _tmp107 = _tmp84 * _tmp90;
|
||||
const Scalar _tmp108 = P(0, 4) * _tmp63 + P(1, 4) * _tmp60 - P(12, 4) * _tmp50 -
|
||||
P(13, 4) * _tmp65 - P(14, 4) * _tmp67 + P(2, 4) * _tmp45 + P(3, 4);
|
||||
const Scalar _tmp108 = P(0, 14) * _tmp91 + P(1, 14) * _tmp99 - P(12, 14) * _tmp100 -
|
||||
const Scalar _tmp109 = P(0, 14) * _tmp91 + P(1, 14) * _tmp99 - P(12, 14) * _tmp100 -
|
||||
P(13, 14) * _tmp95 - P(14, 14) * _tmp96 + P(2, 14) * _tmp92 + P(4, 14);
|
||||
const Scalar _tmp109 = P(0, 0) * _tmp91 + P(1, 0) * _tmp99 - P(12, 0) * _tmp100 -
|
||||
const Scalar _tmp110 = P(0, 0) * _tmp91 + P(1, 0) * _tmp99 - P(12, 0) * _tmp100 -
|
||||
P(13, 0) * _tmp95 - P(14, 0) * _tmp96 + P(2, 0) * _tmp92 + P(4, 0);
|
||||
const Scalar _tmp110 = P(0, 13) * _tmp91 + P(1, 13) * _tmp99 - P(12, 13) * _tmp100 -
|
||||
const Scalar _tmp111 = P(0, 13) * _tmp91 + P(1, 13) * _tmp99 - P(12, 13) * _tmp100 -
|
||||
P(13, 13) * _tmp95 - P(14, 13) * _tmp96 + P(2, 13) * _tmp92 + P(4, 13);
|
||||
const Scalar _tmp111 = P(0, 12) * _tmp91 + P(1, 12) * _tmp99 - P(12, 12) * _tmp100 -
|
||||
const Scalar _tmp112 = P(0, 12) * _tmp91 + P(1, 12) * _tmp99 - P(12, 12) * _tmp100 -
|
||||
P(13, 12) * _tmp95 - P(14, 12) * _tmp96 + P(2, 12) * _tmp92 + P(4, 12);
|
||||
const Scalar _tmp112 = P(0, 1) * _tmp91 + P(1, 1) * _tmp99 - P(12, 1) * _tmp100 -
|
||||
const Scalar _tmp113 = P(0, 1) * _tmp91 + P(1, 1) * _tmp99 - P(12, 1) * _tmp100 -
|
||||
P(13, 1) * _tmp95 - P(14, 1) * _tmp96 + P(2, 1) * _tmp92 + P(4, 1);
|
||||
const Scalar _tmp113 = P(0, 2) * _tmp91 + P(1, 2) * _tmp99 - P(12, 2) * _tmp100 -
|
||||
const Scalar _tmp114 = P(0, 2) * _tmp91 + P(1, 2) * _tmp99 - P(12, 2) * _tmp100 -
|
||||
P(13, 2) * _tmp95 - P(14, 2) * _tmp96 + P(2, 2) * _tmp92 + P(4, 2);
|
||||
const Scalar _tmp114 = P(0, 4) * _tmp91 + P(1, 4) * _tmp99 - P(12, 4) * _tmp100 -
|
||||
const Scalar _tmp115 = P(0, 4) * _tmp91 + P(1, 4) * _tmp99 - P(12, 4) * _tmp100 -
|
||||
P(13, 4) * _tmp95 - P(14, 4) * _tmp96 + P(2, 4) * _tmp92 + P(4, 4);
|
||||
const Scalar _tmp115 =
|
||||
dt * (_tmp44 * (_tmp38 + _tmp40 + _tmp42 + _tmp57) + _tmp59 * (_tmp89 + _tmp98));
|
||||
const Scalar _tmp116 = _tmp48 + _tmp93;
|
||||
const Scalar _tmp117 = _tmp116 * dt;
|
||||
const Scalar _tmp118 = _tmp87 + _tmp88;
|
||||
const Scalar _tmp119 = _tmp118 * dt;
|
||||
const Scalar _tmp120 = dt * (_tmp118 * _tmp35 + _tmp44 * (_tmp51 + _tmp54));
|
||||
const Scalar _tmp121 = _tmp52 + _tmp53;
|
||||
const Scalar _tmp122 = dt * (_tmp121 * _tmp59 + _tmp35 * (_tmp36 + _tmp37 + _tmp41 + _tmp55));
|
||||
const Scalar _tmp123 = _tmp121 * dt;
|
||||
const Scalar _tmp124 = P(0, 5) + P(1, 5) * _tmp5 + P(2, 5) * _tmp2 - P(9, 5) * dt;
|
||||
const Scalar _tmp125 = P(0, 5) * _tmp13 + P(1, 5) - P(10, 5) * dt + P(2, 5) * _tmp16;
|
||||
const Scalar _tmp126 = P(0, 5) * _tmp22 + P(1, 5) * _tmp23 - P(11, 5) * dt + P(2, 5);
|
||||
const Scalar _tmp127 = _tmp118 * _tmp83;
|
||||
const Scalar _tmp116 = dt * (_tmp44 * (_tmp39 + _tmp56) + _tmp59 * (_tmp89 + _tmp98));
|
||||
const Scalar _tmp117 = _tmp48 + _tmp93;
|
||||
const Scalar _tmp118 = _tmp117 * dt;
|
||||
const Scalar _tmp119 = _tmp87 + _tmp88;
|
||||
const Scalar _tmp120 = _tmp119 * dt;
|
||||
const Scalar _tmp121 = dt * (_tmp119 * _tmp35 + _tmp44 * (_tmp51 + _tmp54));
|
||||
const Scalar _tmp122 = _tmp52 + _tmp53;
|
||||
const Scalar _tmp123 = dt * (_tmp122 * _tmp59 + _tmp35 * (_tmp43 + _tmp58));
|
||||
const Scalar _tmp124 = _tmp122 * dt;
|
||||
const Scalar _tmp125 = P(0, 5) + P(1, 5) * _tmp5 + P(2, 5) * _tmp2 - P(9, 5) * dt;
|
||||
const Scalar _tmp126 = P(0, 5) * _tmp13 + P(1, 5) - P(10, 5) * dt + P(2, 5) * _tmp16;
|
||||
const Scalar _tmp127 = P(0, 5) * _tmp22 + P(1, 5) * _tmp23 - P(11, 5) * dt + P(2, 5);
|
||||
const Scalar _tmp128 = P(0, 5) * _tmp63 + P(1, 5) * _tmp60 - P(12, 5) * _tmp50 -
|
||||
P(13, 5) * _tmp65 - P(14, 5) * _tmp67 + P(2, 5) * _tmp45 + P(3, 5);
|
||||
const Scalar _tmp129 = P(0, 5) * _tmp91 + P(1, 5) * _tmp99 - P(12, 5) * _tmp100 -
|
||||
P(13, 5) * _tmp95 - P(14, 5) * _tmp96 + P(2, 5) * _tmp92 + P(4, 5);
|
||||
const Scalar _tmp130 = P(0, 13) * _tmp115 + P(1, 13) * _tmp122 - P(12, 13) * _tmp123 -
|
||||
P(13, 13) * _tmp119 - P(14, 13) * _tmp117 + P(2, 13) * _tmp120 + P(5, 13);
|
||||
const Scalar _tmp131 = P(0, 14) * _tmp115 + P(1, 14) * _tmp122 - P(12, 14) * _tmp123 -
|
||||
P(13, 14) * _tmp119 - P(14, 14) * _tmp117 + P(2, 14) * _tmp120 + P(5, 14);
|
||||
const Scalar _tmp132 = P(0, 12) * _tmp115 + P(1, 12) * _tmp122 - P(12, 12) * _tmp123 -
|
||||
P(13, 12) * _tmp119 - P(14, 12) * _tmp117 + P(2, 12) * _tmp120 + P(5, 12);
|
||||
const Scalar _tmp133 = P(0, 5) * _tmp115 + P(1, 5) * _tmp122 - P(12, 5) * _tmp123 -
|
||||
P(13, 5) * _tmp119 - P(14, 5) * _tmp117 + P(2, 5) * _tmp120 + P(5, 5);
|
||||
const Scalar _tmp130 = P(0, 13) * _tmp116 + P(1, 13) * _tmp123 - P(12, 13) * _tmp124 -
|
||||
P(13, 13) * _tmp120 - P(14, 13) * _tmp118 + P(2, 13) * _tmp121 + P(5, 13);
|
||||
const Scalar _tmp131 = P(0, 14) * _tmp116 + P(1, 14) * _tmp123 - P(12, 14) * _tmp124 -
|
||||
P(13, 14) * _tmp120 - P(14, 14) * _tmp118 + P(2, 14) * _tmp121 + P(5, 14);
|
||||
const Scalar _tmp132 = P(0, 12) * _tmp116 + P(1, 12) * _tmp123 - P(12, 12) * _tmp124 -
|
||||
P(13, 12) * _tmp120 - P(14, 12) * _tmp118 + P(2, 12) * _tmp121 + P(5, 12);
|
||||
const Scalar _tmp133 = P(0, 5) * _tmp116 + P(1, 5) * _tmp123 - P(12, 5) * _tmp124 -
|
||||
P(13, 5) * _tmp120 - P(14, 5) * _tmp118 + P(2, 5) * _tmp121 + P(5, 5);
|
||||
|
||||
// Output terms (1)
|
||||
matrix::Matrix<Scalar, 23, 23> _res;
|
||||
matrix::Matrix<Scalar, 24, 24> _res;
|
||||
|
||||
_res(0, 0) = _tmp10 + _tmp11 + _tmp2 * _tmp6 + _tmp5 * _tmp7 - _tmp8 * dt;
|
||||
_res(1, 0) = 0;
|
||||
|
|
@ -222,6 +222,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 0) = 0;
|
||||
_res(21, 0) = 0;
|
||||
_res(22, 0) = 0;
|
||||
_res(23, 0) = 0;
|
||||
_res(0, 1) = _tmp11 * _tmp13 - _tmp12 * dt + _tmp16 * _tmp6 + _tmp7;
|
||||
_res(1, 1) = _tmp10 + _tmp13 * _tmp18 + _tmp16 * _tmp19 - _tmp17 * dt + _tmp20;
|
||||
_res(2, 1) = 0;
|
||||
|
|
@ -245,6 +246,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 1) = 0;
|
||||
_res(21, 1) = 0;
|
||||
_res(22, 1) = 0;
|
||||
_res(23, 1) = 0;
|
||||
_res(0, 2) = _tmp11 * _tmp22 - _tmp21 * dt + _tmp23 * _tmp7 + _tmp6;
|
||||
_res(1, 2) = _tmp18 * _tmp22 + _tmp19 + _tmp20 * _tmp23 - _tmp24 * dt;
|
||||
_res(2, 2) = _tmp10 + _tmp22 * _tmp27 + _tmp23 * _tmp26 - _tmp25 * dt + _tmp28;
|
||||
|
|
@ -268,6 +270,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 2) = 0;
|
||||
_res(21, 2) = 0;
|
||||
_res(22, 2) = 0;
|
||||
_res(23, 2) = 0;
|
||||
_res(0, 3) = _tmp11 * _tmp63 + _tmp45 * _tmp6 - _tmp46 * _tmp50 + _tmp60 * _tmp7 -
|
||||
_tmp64 * _tmp65 - _tmp66 * _tmp67 + _tmp68;
|
||||
_res(1, 3) = _tmp18 * _tmp63 + _tmp19 * _tmp45 + _tmp20 * _tmp60 - _tmp50 * _tmp71 -
|
||||
|
|
@ -297,17 +300,18 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 3) = 0;
|
||||
_res(21, 3) = 0;
|
||||
_res(22, 3) = 0;
|
||||
_res(23, 3) = 0;
|
||||
_res(0, 4) = -_tmp100 * _tmp46 + _tmp101 + _tmp11 * _tmp91 + _tmp6 * _tmp92 - _tmp64 * _tmp95 -
|
||||
_tmp66 * _tmp96 + _tmp7 * _tmp99;
|
||||
_res(1, 4) = -_tmp100 * _tmp71 + _tmp102 + _tmp18 * _tmp91 + _tmp19 * _tmp92 + _tmp20 * _tmp99 -
|
||||
_tmp69 * _tmp95 - _tmp70 * _tmp96;
|
||||
_res(2, 4) = -_tmp100 * _tmp75 - _tmp103 * _tmp94 + _tmp104 + _tmp26 * _tmp99 + _tmp27 * _tmp91 +
|
||||
_tmp28 * _tmp92 - _tmp73 * _tmp96;
|
||||
_res(3, 4) = -_tmp100 * _tmp85 + _tmp105 * _tmp49 + _tmp106 * _tmp61 + _tmp107 +
|
||||
_tmp34 * _tmp83 * _tmp94 + _tmp77 * _tmp99 - _tmp78 * _tmp96 - _tmp79 * _tmp95 +
|
||||
_tmp80 * _tmp91 + _tmp82 * _tmp92;
|
||||
_res(4, 4) = -_tmp100 * _tmp111 - _tmp108 * _tmp96 + _tmp109 * _tmp91 - _tmp110 * _tmp95 +
|
||||
_tmp112 * _tmp99 + _tmp113 * _tmp92 + _tmp114 +
|
||||
_res(3, 4) = -_tmp100 * _tmp85 + _tmp105 * _tmp49 + _tmp106 * _tmp34 + _tmp107 * _tmp61 +
|
||||
_tmp108 + _tmp77 * _tmp99 - _tmp78 * _tmp96 - _tmp79 * _tmp95 + _tmp80 * _tmp91 +
|
||||
_tmp82 * _tmp92;
|
||||
_res(4, 4) = -_tmp100 * _tmp112 - _tmp109 * _tmp96 + _tmp110 * _tmp91 - _tmp111 * _tmp95 +
|
||||
_tmp113 * _tmp99 + _tmp114 * _tmp92 + _tmp115 +
|
||||
_tmp81 * std::pow(_tmp97, Scalar(2)) + _tmp83 * std::pow(_tmp94, Scalar(2)) +
|
||||
_tmp84 * std::pow(_tmp90, Scalar(2));
|
||||
_res(5, 4) = 0;
|
||||
|
|
@ -328,28 +332,29 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 4) = 0;
|
||||
_res(21, 4) = 0;
|
||||
_res(22, 4) = 0;
|
||||
_res(0, 5) = _tmp11 * _tmp115 - _tmp117 * _tmp66 - _tmp119 * _tmp64 + _tmp120 * _tmp6 +
|
||||
_tmp122 * _tmp7 - _tmp123 * _tmp46 + _tmp124;
|
||||
_res(1, 5) = _tmp115 * _tmp18 - _tmp117 * _tmp70 - _tmp119 * _tmp69 + _tmp120 * _tmp19 +
|
||||
_tmp122 * _tmp20 - _tmp123 * _tmp71 + _tmp125;
|
||||
_res(2, 5) = -_tmp103 * _tmp118 + _tmp115 * _tmp27 - _tmp117 * _tmp73 + _tmp120 * _tmp28 +
|
||||
_tmp122 * _tmp26 - _tmp123 * _tmp75 + _tmp126;
|
||||
_res(3, 5) = _tmp115 * _tmp80 + _tmp116 * _tmp61 * _tmp84 - _tmp117 * _tmp78 - _tmp119 * _tmp79 +
|
||||
_tmp120 * _tmp82 + _tmp121 * _tmp49 * _tmp81 + _tmp122 * _tmp77 - _tmp123 * _tmp85 +
|
||||
_tmp127 * _tmp34 + _tmp128;
|
||||
_res(4, 5) = _tmp105 * _tmp121 + _tmp106 * _tmp116 - _tmp108 * _tmp117 + _tmp109 * _tmp115 -
|
||||
_tmp110 * _tmp119 - _tmp111 * _tmp123 + _tmp112 * _tmp122 + _tmp113 * _tmp120 +
|
||||
_tmp127 * _tmp94 + _tmp129;
|
||||
_res(5, 5) = _tmp115 * (P(0, 0) * _tmp115 + P(1, 0) * _tmp122 - P(12, 0) * _tmp123 -
|
||||
P(13, 0) * _tmp119 - P(14, 0) * _tmp117 + P(2, 0) * _tmp120 + P(5, 0)) +
|
||||
std::pow(_tmp116, Scalar(2)) * _tmp84 - _tmp117 * _tmp131 +
|
||||
std::pow(_tmp118, Scalar(2)) * _tmp83 - _tmp119 * _tmp130 +
|
||||
_tmp120 * (P(0, 2) * _tmp115 + P(1, 2) * _tmp122 - P(12, 2) * _tmp123 -
|
||||
P(13, 2) * _tmp119 - P(14, 2) * _tmp117 + P(2, 2) * _tmp120 + P(5, 2)) +
|
||||
std::pow(_tmp121, Scalar(2)) * _tmp81 +
|
||||
_tmp122 * (P(0, 1) * _tmp115 + P(1, 1) * _tmp122 - P(12, 1) * _tmp123 -
|
||||
P(13, 1) * _tmp119 - P(14, 1) * _tmp117 + P(2, 1) * _tmp120 + P(5, 1)) -
|
||||
_tmp123 * _tmp132 + _tmp133;
|
||||
_res(23, 4) = 0;
|
||||
_res(0, 5) = _tmp11 * _tmp116 - _tmp118 * _tmp66 - _tmp120 * _tmp64 + _tmp121 * _tmp6 +
|
||||
_tmp123 * _tmp7 - _tmp124 * _tmp46 + _tmp125;
|
||||
_res(1, 5) = _tmp116 * _tmp18 - _tmp118 * _tmp70 - _tmp120 * _tmp69 + _tmp121 * _tmp19 +
|
||||
_tmp123 * _tmp20 - _tmp124 * _tmp71 + _tmp126;
|
||||
_res(2, 5) = -_tmp103 * _tmp119 + _tmp116 * _tmp27 - _tmp118 * _tmp73 + _tmp121 * _tmp28 +
|
||||
_tmp123 * _tmp26 - _tmp124 * _tmp75 + _tmp127;
|
||||
_res(3, 5) = _tmp116 * _tmp80 + _tmp117 * _tmp61 * _tmp84 - _tmp118 * _tmp78 +
|
||||
_tmp119 * _tmp34 * _tmp83 - _tmp120 * _tmp79 + _tmp121 * _tmp82 +
|
||||
_tmp122 * _tmp49 * _tmp81 + _tmp123 * _tmp77 - _tmp124 * _tmp85 + _tmp128;
|
||||
_res(4, 5) = _tmp105 * _tmp122 + _tmp106 * _tmp119 + _tmp107 * _tmp117 - _tmp109 * _tmp118 +
|
||||
_tmp110 * _tmp116 - _tmp111 * _tmp120 - _tmp112 * _tmp124 + _tmp113 * _tmp123 +
|
||||
_tmp114 * _tmp121 + _tmp129;
|
||||
_res(5, 5) = _tmp116 * (P(0, 0) * _tmp116 + P(1, 0) * _tmp123 - P(12, 0) * _tmp124 -
|
||||
P(13, 0) * _tmp120 - P(14, 0) * _tmp118 + P(2, 0) * _tmp121 + P(5, 0)) +
|
||||
std::pow(_tmp117, Scalar(2)) * _tmp84 - _tmp118 * _tmp131 +
|
||||
std::pow(_tmp119, Scalar(2)) * _tmp83 - _tmp120 * _tmp130 +
|
||||
_tmp121 * (P(0, 2) * _tmp116 + P(1, 2) * _tmp123 - P(12, 2) * _tmp124 -
|
||||
P(13, 2) * _tmp120 - P(14, 2) * _tmp118 + P(2, 2) * _tmp121 + P(5, 2)) +
|
||||
std::pow(_tmp122, Scalar(2)) * _tmp81 +
|
||||
_tmp123 * (P(0, 1) * _tmp116 + P(1, 1) * _tmp123 - P(12, 1) * _tmp124 -
|
||||
P(13, 1) * _tmp120 - P(14, 1) * _tmp118 + P(2, 1) * _tmp121 + P(5, 1)) -
|
||||
_tmp124 * _tmp132 + _tmp133;
|
||||
_res(6, 5) = 0;
|
||||
_res(7, 5) = 0;
|
||||
_res(8, 5) = 0;
|
||||
|
|
@ -367,6 +372,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 5) = 0;
|
||||
_res(21, 5) = 0;
|
||||
_res(22, 5) = 0;
|
||||
_res(23, 5) = 0;
|
||||
_res(0, 6) = P(0, 6) + P(1, 6) * _tmp5 + P(2, 6) * _tmp2 - P(9, 6) * dt + _tmp68 * dt;
|
||||
_res(1, 6) = P(0, 6) * _tmp13 + P(1, 6) - P(10, 6) * dt + P(2, 6) * _tmp16 + _tmp72 * dt;
|
||||
_res(2, 6) = P(0, 6) * _tmp22 + P(1, 6) * _tmp23 - P(11, 6) * dt + P(2, 6) + _tmp76 * dt;
|
||||
|
|
@ -376,10 +382,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
P(14, 6) * _tmp96 + P(2, 6) * _tmp92 + P(4, 6) +
|
||||
dt * (P(0, 3) * _tmp91 + P(1, 3) * _tmp99 - P(12, 3) * _tmp100 - P(13, 3) * _tmp95 -
|
||||
P(14, 3) * _tmp96 + P(2, 3) * _tmp92 + P(4, 3));
|
||||
_res(5, 6) = P(0, 6) * _tmp115 + P(1, 6) * _tmp122 - P(12, 6) * _tmp123 - P(13, 6) * _tmp119 -
|
||||
P(14, 6) * _tmp117 + P(2, 6) * _tmp120 + P(5, 6) +
|
||||
dt * (P(0, 3) * _tmp115 + P(1, 3) * _tmp122 - P(12, 3) * _tmp123 -
|
||||
P(13, 3) * _tmp119 - P(14, 3) * _tmp117 + P(2, 3) * _tmp120 + P(5, 3));
|
||||
_res(5, 6) = P(0, 6) * _tmp116 + P(1, 6) * _tmp123 - P(12, 6) * _tmp124 - P(13, 6) * _tmp120 -
|
||||
P(14, 6) * _tmp118 + P(2, 6) * _tmp121 + P(5, 6) +
|
||||
dt * (P(0, 3) * _tmp116 + P(1, 3) * _tmp123 - P(12, 3) * _tmp124 -
|
||||
P(13, 3) * _tmp120 - P(14, 3) * _tmp118 + P(2, 3) * _tmp121 + P(5, 3));
|
||||
_res(6, 6) = P(3, 6) * dt + P(6, 6) + dt * (P(3, 3) * dt + P(6, 3));
|
||||
_res(7, 6) = 0;
|
||||
_res(8, 6) = 0;
|
||||
|
|
@ -397,17 +403,18 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 6) = 0;
|
||||
_res(21, 6) = 0;
|
||||
_res(22, 6) = 0;
|
||||
_res(23, 6) = 0;
|
||||
_res(0, 7) = P(0, 7) + P(1, 7) * _tmp5 + P(2, 7) * _tmp2 - P(9, 7) * dt + _tmp101 * dt;
|
||||
_res(1, 7) = P(0, 7) * _tmp13 + P(1, 7) - P(10, 7) * dt + P(2, 7) * _tmp16 + _tmp102 * dt;
|
||||
_res(2, 7) = P(0, 7) * _tmp22 + P(1, 7) * _tmp23 - P(11, 7) * dt + P(2, 7) + _tmp104 * dt;
|
||||
_res(3, 7) = P(0, 7) * _tmp63 + P(1, 7) * _tmp60 - P(12, 7) * _tmp50 - P(13, 7) * _tmp65 -
|
||||
P(14, 7) * _tmp67 + P(2, 7) * _tmp45 + P(3, 7) + _tmp107 * dt;
|
||||
P(14, 7) * _tmp67 + P(2, 7) * _tmp45 + P(3, 7) + _tmp108 * dt;
|
||||
_res(4, 7) = P(0, 7) * _tmp91 + P(1, 7) * _tmp99 - P(12, 7) * _tmp100 - P(13, 7) * _tmp95 -
|
||||
P(14, 7) * _tmp96 + P(2, 7) * _tmp92 + P(4, 7) + _tmp114 * dt;
|
||||
_res(5, 7) = P(0, 7) * _tmp115 + P(1, 7) * _tmp122 - P(12, 7) * _tmp123 - P(13, 7) * _tmp119 -
|
||||
P(14, 7) * _tmp117 + P(2, 7) * _tmp120 + P(5, 7) +
|
||||
dt * (P(0, 4) * _tmp115 + P(1, 4) * _tmp122 - P(12, 4) * _tmp123 -
|
||||
P(13, 4) * _tmp119 - P(14, 4) * _tmp117 + P(2, 4) * _tmp120 + P(5, 4));
|
||||
P(14, 7) * _tmp96 + P(2, 7) * _tmp92 + P(4, 7) + _tmp115 * dt;
|
||||
_res(5, 7) = P(0, 7) * _tmp116 + P(1, 7) * _tmp123 - P(12, 7) * _tmp124 - P(13, 7) * _tmp120 -
|
||||
P(14, 7) * _tmp118 + P(2, 7) * _tmp121 + P(5, 7) +
|
||||
dt * (P(0, 4) * _tmp116 + P(1, 4) * _tmp123 - P(12, 4) * _tmp124 -
|
||||
P(13, 4) * _tmp120 - P(14, 4) * _tmp118 + P(2, 4) * _tmp121 + P(5, 4));
|
||||
_res(6, 7) = P(3, 7) * dt + P(6, 7) + dt * (P(3, 4) * dt + P(6, 4));
|
||||
_res(7, 7) = P(4, 7) * dt + P(7, 7) + dt * (P(4, 4) * dt + P(7, 4));
|
||||
_res(8, 7) = 0;
|
||||
|
|
@ -425,15 +432,16 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 7) = 0;
|
||||
_res(21, 7) = 0;
|
||||
_res(22, 7) = 0;
|
||||
_res(0, 8) = P(0, 8) + P(1, 8) * _tmp5 + P(2, 8) * _tmp2 - P(9, 8) * dt + _tmp124 * dt;
|
||||
_res(1, 8) = P(0, 8) * _tmp13 + P(1, 8) - P(10, 8) * dt + P(2, 8) * _tmp16 + _tmp125 * dt;
|
||||
_res(2, 8) = P(0, 8) * _tmp22 + P(1, 8) * _tmp23 - P(11, 8) * dt + P(2, 8) + _tmp126 * dt;
|
||||
_res(23, 7) = 0;
|
||||
_res(0, 8) = P(0, 8) + P(1, 8) * _tmp5 + P(2, 8) * _tmp2 - P(9, 8) * dt + _tmp125 * dt;
|
||||
_res(1, 8) = P(0, 8) * _tmp13 + P(1, 8) - P(10, 8) * dt + P(2, 8) * _tmp16 + _tmp126 * dt;
|
||||
_res(2, 8) = P(0, 8) * _tmp22 + P(1, 8) * _tmp23 - P(11, 8) * dt + P(2, 8) + _tmp127 * dt;
|
||||
_res(3, 8) = P(0, 8) * _tmp63 + P(1, 8) * _tmp60 - P(12, 8) * _tmp50 - P(13, 8) * _tmp65 -
|
||||
P(14, 8) * _tmp67 + P(2, 8) * _tmp45 + P(3, 8) + _tmp128 * dt;
|
||||
_res(4, 8) = P(0, 8) * _tmp91 + P(1, 8) * _tmp99 - P(12, 8) * _tmp100 - P(13, 8) * _tmp95 -
|
||||
P(14, 8) * _tmp96 + P(2, 8) * _tmp92 + P(4, 8) + _tmp129 * dt;
|
||||
_res(5, 8) = P(0, 8) * _tmp115 + P(1, 8) * _tmp122 - P(12, 8) * _tmp123 - P(13, 8) * _tmp119 -
|
||||
P(14, 8) * _tmp117 + P(2, 8) * _tmp120 + P(5, 8) + _tmp133 * dt;
|
||||
_res(5, 8) = P(0, 8) * _tmp116 + P(1, 8) * _tmp123 - P(12, 8) * _tmp124 - P(13, 8) * _tmp120 -
|
||||
P(14, 8) * _tmp118 + P(2, 8) * _tmp121 + P(5, 8) + _tmp133 * dt;
|
||||
_res(6, 8) = P(3, 8) * dt + P(6, 8) + dt * (P(3, 5) * dt + P(6, 5));
|
||||
_res(7, 8) = P(4, 8) * dt + P(7, 8) + dt * (P(4, 5) * dt + P(7, 5));
|
||||
_res(8, 8) = P(5, 8) * dt + P(8, 8) + dt * (P(5, 5) * dt + P(8, 5));
|
||||
|
|
@ -451,6 +459,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 8) = 0;
|
||||
_res(21, 8) = 0;
|
||||
_res(22, 8) = 0;
|
||||
_res(23, 8) = 0;
|
||||
_res(0, 9) = _tmp8;
|
||||
_res(1, 9) = P(0, 9) * _tmp13 + P(1, 9) - P(10, 9) * dt + P(2, 9) * _tmp16;
|
||||
_res(2, 9) = P(0, 9) * _tmp22 + P(1, 9) * _tmp23 - P(11, 9) * dt + P(2, 9);
|
||||
|
|
@ -458,8 +467,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
P(14, 9) * _tmp67 + P(2, 9) * _tmp45 + P(3, 9);
|
||||
_res(4, 9) = P(0, 9) * _tmp91 + P(1, 9) * _tmp99 - P(12, 9) * _tmp100 - P(13, 9) * _tmp95 -
|
||||
P(14, 9) * _tmp96 + P(2, 9) * _tmp92 + P(4, 9);
|
||||
_res(5, 9) = P(0, 9) * _tmp115 + P(1, 9) * _tmp122 - P(12, 9) * _tmp123 - P(13, 9) * _tmp119 -
|
||||
P(14, 9) * _tmp117 + P(2, 9) * _tmp120 + P(5, 9);
|
||||
_res(5, 9) = P(0, 9) * _tmp116 + P(1, 9) * _tmp123 - P(12, 9) * _tmp124 - P(13, 9) * _tmp120 -
|
||||
P(14, 9) * _tmp118 + P(2, 9) * _tmp121 + P(5, 9);
|
||||
_res(6, 9) = P(3, 9) * dt + P(6, 9);
|
||||
_res(7, 9) = P(4, 9) * dt + P(7, 9);
|
||||
_res(8, 9) = P(5, 9) * dt + P(8, 9);
|
||||
|
|
@ -477,6 +486,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 9) = 0;
|
||||
_res(21, 9) = 0;
|
||||
_res(22, 9) = 0;
|
||||
_res(23, 9) = 0;
|
||||
_res(0, 10) = _tmp12;
|
||||
_res(1, 10) = _tmp17;
|
||||
_res(2, 10) = P(0, 10) * _tmp22 + P(1, 10) * _tmp23 - P(11, 10) * dt + P(2, 10);
|
||||
|
|
@ -484,8 +494,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
P(14, 10) * _tmp67 + P(2, 10) * _tmp45 + P(3, 10);
|
||||
_res(4, 10) = P(0, 10) * _tmp91 + P(1, 10) * _tmp99 - P(12, 10) * _tmp100 - P(13, 10) * _tmp95 -
|
||||
P(14, 10) * _tmp96 + P(2, 10) * _tmp92 + P(4, 10);
|
||||
_res(5, 10) = P(0, 10) * _tmp115 + P(1, 10) * _tmp122 - P(12, 10) * _tmp123 -
|
||||
P(13, 10) * _tmp119 - P(14, 10) * _tmp117 + P(2, 10) * _tmp120 + P(5, 10);
|
||||
_res(5, 10) = P(0, 10) * _tmp116 + P(1, 10) * _tmp123 - P(12, 10) * _tmp124 -
|
||||
P(13, 10) * _tmp120 - P(14, 10) * _tmp118 + P(2, 10) * _tmp121 + P(5, 10);
|
||||
_res(6, 10) = P(3, 10) * dt + P(6, 10);
|
||||
_res(7, 10) = P(4, 10) * dt + P(7, 10);
|
||||
_res(8, 10) = P(5, 10) * dt + P(8, 10);
|
||||
|
|
@ -503,6 +513,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 10) = 0;
|
||||
_res(21, 10) = 0;
|
||||
_res(22, 10) = 0;
|
||||
_res(23, 10) = 0;
|
||||
_res(0, 11) = _tmp21;
|
||||
_res(1, 11) = _tmp24;
|
||||
_res(2, 11) = _tmp25;
|
||||
|
|
@ -510,8 +521,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
P(14, 11) * _tmp67 + P(2, 11) * _tmp45 + P(3, 11);
|
||||
_res(4, 11) = P(0, 11) * _tmp91 + P(1, 11) * _tmp99 - P(12, 11) * _tmp100 - P(13, 11) * _tmp95 -
|
||||
P(14, 11) * _tmp96 + P(2, 11) * _tmp92 + P(4, 11);
|
||||
_res(5, 11) = P(0, 11) * _tmp115 + P(1, 11) * _tmp122 - P(12, 11) * _tmp123 -
|
||||
P(13, 11) * _tmp119 - P(14, 11) * _tmp117 + P(2, 11) * _tmp120 + P(5, 11);
|
||||
_res(5, 11) = P(0, 11) * _tmp116 + P(1, 11) * _tmp123 - P(12, 11) * _tmp124 -
|
||||
P(13, 11) * _tmp120 - P(14, 11) * _tmp118 + P(2, 11) * _tmp121 + P(5, 11);
|
||||
_res(6, 11) = P(3, 11) * dt + P(6, 11);
|
||||
_res(7, 11) = P(4, 11) * dt + P(7, 11);
|
||||
_res(8, 11) = P(5, 11) * dt + P(8, 11);
|
||||
|
|
@ -529,11 +540,12 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 11) = 0;
|
||||
_res(21, 11) = 0;
|
||||
_res(22, 11) = 0;
|
||||
_res(23, 11) = 0;
|
||||
_res(0, 12) = _tmp46;
|
||||
_res(1, 12) = _tmp71;
|
||||
_res(2, 12) = _tmp75;
|
||||
_res(3, 12) = _tmp85;
|
||||
_res(4, 12) = _tmp111;
|
||||
_res(4, 12) = _tmp112;
|
||||
_res(5, 12) = _tmp132;
|
||||
_res(6, 12) = P(3, 12) * dt + P(6, 12);
|
||||
_res(7, 12) = P(4, 12) * dt + P(7, 12);
|
||||
|
|
@ -552,11 +564,12 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 12) = 0;
|
||||
_res(21, 12) = 0;
|
||||
_res(22, 12) = 0;
|
||||
_res(23, 12) = 0;
|
||||
_res(0, 13) = _tmp64;
|
||||
_res(1, 13) = _tmp69;
|
||||
_res(2, 13) = _tmp74;
|
||||
_res(3, 13) = _tmp79;
|
||||
_res(4, 13) = _tmp110;
|
||||
_res(4, 13) = _tmp111;
|
||||
_res(5, 13) = _tmp130;
|
||||
_res(6, 13) = P(3, 13) * dt + P(6, 13);
|
||||
_res(7, 13) = P(4, 13) * dt + P(7, 13);
|
||||
|
|
@ -575,11 +588,12 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 13) = 0;
|
||||
_res(21, 13) = 0;
|
||||
_res(22, 13) = 0;
|
||||
_res(23, 13) = 0;
|
||||
_res(0, 14) = _tmp66;
|
||||
_res(1, 14) = _tmp70;
|
||||
_res(2, 14) = _tmp73;
|
||||
_res(3, 14) = _tmp78;
|
||||
_res(4, 14) = _tmp108;
|
||||
_res(4, 14) = _tmp109;
|
||||
_res(5, 14) = _tmp131;
|
||||
_res(6, 14) = P(3, 14) * dt + P(6, 14);
|
||||
_res(7, 14) = P(4, 14) * dt + P(7, 14);
|
||||
|
|
@ -598,6 +612,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 14) = 0;
|
||||
_res(21, 14) = 0;
|
||||
_res(22, 14) = 0;
|
||||
_res(23, 14) = 0;
|
||||
_res(0, 15) = P(0, 15) + P(1, 15) * _tmp5 + P(2, 15) * _tmp2 - P(9, 15) * dt;
|
||||
_res(1, 15) = P(0, 15) * _tmp13 + P(1, 15) - P(10, 15) * dt + P(2, 15) * _tmp16;
|
||||
_res(2, 15) = P(0, 15) * _tmp22 + P(1, 15) * _tmp23 - P(11, 15) * dt + P(2, 15);
|
||||
|
|
@ -605,8 +620,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
P(14, 15) * _tmp67 + P(2, 15) * _tmp45 + P(3, 15);
|
||||
_res(4, 15) = P(0, 15) * _tmp91 + P(1, 15) * _tmp99 - P(12, 15) * _tmp100 - P(13, 15) * _tmp95 -
|
||||
P(14, 15) * _tmp96 + P(2, 15) * _tmp92 + P(4, 15);
|
||||
_res(5, 15) = P(0, 15) * _tmp115 + P(1, 15) * _tmp122 - P(12, 15) * _tmp123 -
|
||||
P(13, 15) * _tmp119 - P(14, 15) * _tmp117 + P(2, 15) * _tmp120 + P(5, 15);
|
||||
_res(5, 15) = P(0, 15) * _tmp116 + P(1, 15) * _tmp123 - P(12, 15) * _tmp124 -
|
||||
P(13, 15) * _tmp120 - P(14, 15) * _tmp118 + P(2, 15) * _tmp121 + P(5, 15);
|
||||
_res(6, 15) = P(3, 15) * dt + P(6, 15);
|
||||
_res(7, 15) = P(4, 15) * dt + P(7, 15);
|
||||
_res(8, 15) = P(5, 15) * dt + P(8, 15);
|
||||
|
|
@ -624,6 +639,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 15) = 0;
|
||||
_res(21, 15) = 0;
|
||||
_res(22, 15) = 0;
|
||||
_res(23, 15) = 0;
|
||||
_res(0, 16) = P(0, 16) + P(1, 16) * _tmp5 + P(2, 16) * _tmp2 - P(9, 16) * dt;
|
||||
_res(1, 16) = P(0, 16) * _tmp13 + P(1, 16) - P(10, 16) * dt + P(2, 16) * _tmp16;
|
||||
_res(2, 16) = P(0, 16) * _tmp22 + P(1, 16) * _tmp23 - P(11, 16) * dt + P(2, 16);
|
||||
|
|
@ -631,8 +647,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
P(14, 16) * _tmp67 + P(2, 16) * _tmp45 + P(3, 16);
|
||||
_res(4, 16) = P(0, 16) * _tmp91 + P(1, 16) * _tmp99 - P(12, 16) * _tmp100 - P(13, 16) * _tmp95 -
|
||||
P(14, 16) * _tmp96 + P(2, 16) * _tmp92 + P(4, 16);
|
||||
_res(5, 16) = P(0, 16) * _tmp115 + P(1, 16) * _tmp122 - P(12, 16) * _tmp123 -
|
||||
P(13, 16) * _tmp119 - P(14, 16) * _tmp117 + P(2, 16) * _tmp120 + P(5, 16);
|
||||
_res(5, 16) = P(0, 16) * _tmp116 + P(1, 16) * _tmp123 - P(12, 16) * _tmp124 -
|
||||
P(13, 16) * _tmp120 - P(14, 16) * _tmp118 + P(2, 16) * _tmp121 + P(5, 16);
|
||||
_res(6, 16) = P(3, 16) * dt + P(6, 16);
|
||||
_res(7, 16) = P(4, 16) * dt + P(7, 16);
|
||||
_res(8, 16) = P(5, 16) * dt + P(8, 16);
|
||||
|
|
@ -650,6 +666,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 16) = 0;
|
||||
_res(21, 16) = 0;
|
||||
_res(22, 16) = 0;
|
||||
_res(23, 16) = 0;
|
||||
_res(0, 17) = P(0, 17) + P(1, 17) * _tmp5 + P(2, 17) * _tmp2 - P(9, 17) * dt;
|
||||
_res(1, 17) = P(0, 17) * _tmp13 + P(1, 17) - P(10, 17) * dt + P(2, 17) * _tmp16;
|
||||
_res(2, 17) = P(0, 17) * _tmp22 + P(1, 17) * _tmp23 - P(11, 17) * dt + P(2, 17);
|
||||
|
|
@ -657,8 +674,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
P(14, 17) * _tmp67 + P(2, 17) * _tmp45 + P(3, 17);
|
||||
_res(4, 17) = P(0, 17) * _tmp91 + P(1, 17) * _tmp99 - P(12, 17) * _tmp100 - P(13, 17) * _tmp95 -
|
||||
P(14, 17) * _tmp96 + P(2, 17) * _tmp92 + P(4, 17);
|
||||
_res(5, 17) = P(0, 17) * _tmp115 + P(1, 17) * _tmp122 - P(12, 17) * _tmp123 -
|
||||
P(13, 17) * _tmp119 - P(14, 17) * _tmp117 + P(2, 17) * _tmp120 + P(5, 17);
|
||||
_res(5, 17) = P(0, 17) * _tmp116 + P(1, 17) * _tmp123 - P(12, 17) * _tmp124 -
|
||||
P(13, 17) * _tmp120 - P(14, 17) * _tmp118 + P(2, 17) * _tmp121 + P(5, 17);
|
||||
_res(6, 17) = P(3, 17) * dt + P(6, 17);
|
||||
_res(7, 17) = P(4, 17) * dt + P(7, 17);
|
||||
_res(8, 17) = P(5, 17) * dt + P(8, 17);
|
||||
|
|
@ -676,6 +693,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 17) = 0;
|
||||
_res(21, 17) = 0;
|
||||
_res(22, 17) = 0;
|
||||
_res(23, 17) = 0;
|
||||
_res(0, 18) = P(0, 18) + P(1, 18) * _tmp5 + P(2, 18) * _tmp2 - P(9, 18) * dt;
|
||||
_res(1, 18) = P(0, 18) * _tmp13 + P(1, 18) - P(10, 18) * dt + P(2, 18) * _tmp16;
|
||||
_res(2, 18) = P(0, 18) * _tmp22 + P(1, 18) * _tmp23 - P(11, 18) * dt + P(2, 18);
|
||||
|
|
@ -683,8 +701,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
P(14, 18) * _tmp67 + P(2, 18) * _tmp45 + P(3, 18);
|
||||
_res(4, 18) = P(0, 18) * _tmp91 + P(1, 18) * _tmp99 - P(12, 18) * _tmp100 - P(13, 18) * _tmp95 -
|
||||
P(14, 18) * _tmp96 + P(2, 18) * _tmp92 + P(4, 18);
|
||||
_res(5, 18) = P(0, 18) * _tmp115 + P(1, 18) * _tmp122 - P(12, 18) * _tmp123 -
|
||||
P(13, 18) * _tmp119 - P(14, 18) * _tmp117 + P(2, 18) * _tmp120 + P(5, 18);
|
||||
_res(5, 18) = P(0, 18) * _tmp116 + P(1, 18) * _tmp123 - P(12, 18) * _tmp124 -
|
||||
P(13, 18) * _tmp120 - P(14, 18) * _tmp118 + P(2, 18) * _tmp121 + P(5, 18);
|
||||
_res(6, 18) = P(3, 18) * dt + P(6, 18);
|
||||
_res(7, 18) = P(4, 18) * dt + P(7, 18);
|
||||
_res(8, 18) = P(5, 18) * dt + P(8, 18);
|
||||
|
|
@ -702,6 +720,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 18) = 0;
|
||||
_res(21, 18) = 0;
|
||||
_res(22, 18) = 0;
|
||||
_res(23, 18) = 0;
|
||||
_res(0, 19) = P(0, 19) + P(1, 19) * _tmp5 + P(2, 19) * _tmp2 - P(9, 19) * dt;
|
||||
_res(1, 19) = P(0, 19) * _tmp13 + P(1, 19) - P(10, 19) * dt + P(2, 19) * _tmp16;
|
||||
_res(2, 19) = P(0, 19) * _tmp22 + P(1, 19) * _tmp23 - P(11, 19) * dt + P(2, 19);
|
||||
|
|
@ -709,8 +728,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
P(14, 19) * _tmp67 + P(2, 19) * _tmp45 + P(3, 19);
|
||||
_res(4, 19) = P(0, 19) * _tmp91 + P(1, 19) * _tmp99 - P(12, 19) * _tmp100 - P(13, 19) * _tmp95 -
|
||||
P(14, 19) * _tmp96 + P(2, 19) * _tmp92 + P(4, 19);
|
||||
_res(5, 19) = P(0, 19) * _tmp115 + P(1, 19) * _tmp122 - P(12, 19) * _tmp123 -
|
||||
P(13, 19) * _tmp119 - P(14, 19) * _tmp117 + P(2, 19) * _tmp120 + P(5, 19);
|
||||
_res(5, 19) = P(0, 19) * _tmp116 + P(1, 19) * _tmp123 - P(12, 19) * _tmp124 -
|
||||
P(13, 19) * _tmp120 - P(14, 19) * _tmp118 + P(2, 19) * _tmp121 + P(5, 19);
|
||||
_res(6, 19) = P(3, 19) * dt + P(6, 19);
|
||||
_res(7, 19) = P(4, 19) * dt + P(7, 19);
|
||||
_res(8, 19) = P(5, 19) * dt + P(8, 19);
|
||||
|
|
@ -728,6 +747,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 19) = 0;
|
||||
_res(21, 19) = 0;
|
||||
_res(22, 19) = 0;
|
||||
_res(23, 19) = 0;
|
||||
_res(0, 20) = P(0, 20) + P(1, 20) * _tmp5 + P(2, 20) * _tmp2 - P(9, 20) * dt;
|
||||
_res(1, 20) = P(0, 20) * _tmp13 + P(1, 20) - P(10, 20) * dt + P(2, 20) * _tmp16;
|
||||
_res(2, 20) = P(0, 20) * _tmp22 + P(1, 20) * _tmp23 - P(11, 20) * dt + P(2, 20);
|
||||
|
|
@ -735,8 +755,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
P(14, 20) * _tmp67 + P(2, 20) * _tmp45 + P(3, 20);
|
||||
_res(4, 20) = P(0, 20) * _tmp91 + P(1, 20) * _tmp99 - P(12, 20) * _tmp100 - P(13, 20) * _tmp95 -
|
||||
P(14, 20) * _tmp96 + P(2, 20) * _tmp92 + P(4, 20);
|
||||
_res(5, 20) = P(0, 20) * _tmp115 + P(1, 20) * _tmp122 - P(12, 20) * _tmp123 -
|
||||
P(13, 20) * _tmp119 - P(14, 20) * _tmp117 + P(2, 20) * _tmp120 + P(5, 20);
|
||||
_res(5, 20) = P(0, 20) * _tmp116 + P(1, 20) * _tmp123 - P(12, 20) * _tmp124 -
|
||||
P(13, 20) * _tmp120 - P(14, 20) * _tmp118 + P(2, 20) * _tmp121 + P(5, 20);
|
||||
_res(6, 20) = P(3, 20) * dt + P(6, 20);
|
||||
_res(7, 20) = P(4, 20) * dt + P(7, 20);
|
||||
_res(8, 20) = P(5, 20) * dt + P(8, 20);
|
||||
|
|
@ -754,6 +774,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 20) = P(20, 20);
|
||||
_res(21, 20) = 0;
|
||||
_res(22, 20) = 0;
|
||||
_res(23, 20) = 0;
|
||||
_res(0, 21) = P(0, 21) + P(1, 21) * _tmp5 + P(2, 21) * _tmp2 - P(9, 21) * dt;
|
||||
_res(1, 21) = P(0, 21) * _tmp13 + P(1, 21) - P(10, 21) * dt + P(2, 21) * _tmp16;
|
||||
_res(2, 21) = P(0, 21) * _tmp22 + P(1, 21) * _tmp23 - P(11, 21) * dt + P(2, 21);
|
||||
|
|
@ -761,8 +782,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
P(14, 21) * _tmp67 + P(2, 21) * _tmp45 + P(3, 21);
|
||||
_res(4, 21) = P(0, 21) * _tmp91 + P(1, 21) * _tmp99 - P(12, 21) * _tmp100 - P(13, 21) * _tmp95 -
|
||||
P(14, 21) * _tmp96 + P(2, 21) * _tmp92 + P(4, 21);
|
||||
_res(5, 21) = P(0, 21) * _tmp115 + P(1, 21) * _tmp122 - P(12, 21) * _tmp123 -
|
||||
P(13, 21) * _tmp119 - P(14, 21) * _tmp117 + P(2, 21) * _tmp120 + P(5, 21);
|
||||
_res(5, 21) = P(0, 21) * _tmp116 + P(1, 21) * _tmp123 - P(12, 21) * _tmp124 -
|
||||
P(13, 21) * _tmp120 - P(14, 21) * _tmp118 + P(2, 21) * _tmp121 + P(5, 21);
|
||||
_res(6, 21) = P(3, 21) * dt + P(6, 21);
|
||||
_res(7, 21) = P(4, 21) * dt + P(7, 21);
|
||||
_res(8, 21) = P(5, 21) * dt + P(8, 21);
|
||||
|
|
@ -780,6 +801,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 21) = P(20, 21);
|
||||
_res(21, 21) = P(21, 21);
|
||||
_res(22, 21) = 0;
|
||||
_res(23, 21) = 0;
|
||||
_res(0, 22) = P(0, 22) + P(1, 22) * _tmp5 + P(2, 22) * _tmp2 - P(9, 22) * dt;
|
||||
_res(1, 22) = P(0, 22) * _tmp13 + P(1, 22) - P(10, 22) * dt + P(2, 22) * _tmp16;
|
||||
_res(2, 22) = P(0, 22) * _tmp22 + P(1, 22) * _tmp23 - P(11, 22) * dt + P(2, 22);
|
||||
|
|
@ -787,8 +809,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
P(14, 22) * _tmp67 + P(2, 22) * _tmp45 + P(3, 22);
|
||||
_res(4, 22) = P(0, 22) * _tmp91 + P(1, 22) * _tmp99 - P(12, 22) * _tmp100 - P(13, 22) * _tmp95 -
|
||||
P(14, 22) * _tmp96 + P(2, 22) * _tmp92 + P(4, 22);
|
||||
_res(5, 22) = P(0, 22) * _tmp115 + P(1, 22) * _tmp122 - P(12, 22) * _tmp123 -
|
||||
P(13, 22) * _tmp119 - P(14, 22) * _tmp117 + P(2, 22) * _tmp120 + P(5, 22);
|
||||
_res(5, 22) = P(0, 22) * _tmp116 + P(1, 22) * _tmp123 - P(12, 22) * _tmp124 -
|
||||
P(13, 22) * _tmp120 - P(14, 22) * _tmp118 + P(2, 22) * _tmp121 + P(5, 22);
|
||||
_res(6, 22) = P(3, 22) * dt + P(6, 22);
|
||||
_res(7, 22) = P(4, 22) * dt + P(7, 22);
|
||||
_res(8, 22) = P(5, 22) * dt + P(8, 22);
|
||||
|
|
@ -806,6 +828,34 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
|||
_res(20, 22) = P(20, 22);
|
||||
_res(21, 22) = P(21, 22);
|
||||
_res(22, 22) = P(22, 22);
|
||||
_res(23, 22) = 0;
|
||||
_res(0, 23) = P(0, 23) + P(1, 23) * _tmp5 + P(2, 23) * _tmp2 - P(9, 23) * dt;
|
||||
_res(1, 23) = P(0, 23) * _tmp13 + P(1, 23) - P(10, 23) * dt + P(2, 23) * _tmp16;
|
||||
_res(2, 23) = P(0, 23) * _tmp22 + P(1, 23) * _tmp23 - P(11, 23) * dt + P(2, 23);
|
||||
_res(3, 23) = P(0, 23) * _tmp63 + P(1, 23) * _tmp60 - P(12, 23) * _tmp50 - P(13, 23) * _tmp65 -
|
||||
P(14, 23) * _tmp67 + P(2, 23) * _tmp45 + P(3, 23);
|
||||
_res(4, 23) = P(0, 23) * _tmp91 + P(1, 23) * _tmp99 - P(12, 23) * _tmp100 - P(13, 23) * _tmp95 -
|
||||
P(14, 23) * _tmp96 + P(2, 23) * _tmp92 + P(4, 23);
|
||||
_res(5, 23) = P(0, 23) * _tmp116 + P(1, 23) * _tmp123 - P(12, 23) * _tmp124 -
|
||||
P(13, 23) * _tmp120 - P(14, 23) * _tmp118 + P(2, 23) * _tmp121 + P(5, 23);
|
||||
_res(6, 23) = P(3, 23) * dt + P(6, 23);
|
||||
_res(7, 23) = P(4, 23) * dt + P(7, 23);
|
||||
_res(8, 23) = P(5, 23) * dt + P(8, 23);
|
||||
_res(9, 23) = P(9, 23);
|
||||
_res(10, 23) = P(10, 23);
|
||||
_res(11, 23) = P(11, 23);
|
||||
_res(12, 23) = P(12, 23);
|
||||
_res(13, 23) = P(13, 23);
|
||||
_res(14, 23) = P(14, 23);
|
||||
_res(15, 23) = P(15, 23);
|
||||
_res(16, 23) = P(16, 23);
|
||||
_res(17, 23) = P(17, 23);
|
||||
_res(18, 23) = P(18, 23);
|
||||
_res(19, 23) = P(19, 23);
|
||||
_res(20, 23) = P(20, 23);
|
||||
_res(21, 23) = P(21, 23);
|
||||
_res(22, 23) = P(22, 23);
|
||||
_res(23, 23) = P(23, 23);
|
||||
|
||||
return _res;
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
|
|
|||
|
|
@ -18,9 +18,10 @@ struct StateSample {
|
|||
matrix::Vector3<float> mag_I{};
|
||||
matrix::Vector3<float> mag_B{};
|
||||
matrix::Vector2<float> wind_vel{};
|
||||
float terrain_vpos{};
|
||||
|
||||
matrix::Vector<float, 24> Data() const {
|
||||
matrix::Vector<float, 24> state;
|
||||
matrix::Vector<float, 25> Data() const {
|
||||
matrix::Vector<float, 25> state;
|
||||
state.slice<4, 1>(0, 0) = quat_nominal;
|
||||
state.slice<3, 1>(4, 0) = vel;
|
||||
state.slice<3, 1>(7, 0) = pos;
|
||||
|
|
@ -29,15 +30,16 @@ struct StateSample {
|
|||
state.slice<3, 1>(16, 0) = mag_I;
|
||||
state.slice<3, 1>(19, 0) = mag_B;
|
||||
state.slice<2, 1>(22, 0) = wind_vel;
|
||||
state.slice<1, 1>(24, 0) = terrain_vpos;
|
||||
return state;
|
||||
};
|
||||
|
||||
const matrix::Vector<float, 24>& vector() const {
|
||||
return *reinterpret_cast<matrix::Vector<float, 24>*>(const_cast<float*>(reinterpret_cast<const float*>(&quat_nominal)));
|
||||
const matrix::Vector<float, 25>& vector() const {
|
||||
return *reinterpret_cast<matrix::Vector<float, 25>*>(const_cast<float*>(reinterpret_cast<const float*>(&quat_nominal)));
|
||||
};
|
||||
|
||||
};
|
||||
static_assert(sizeof(matrix::Vector<float, 24>) == sizeof(StateSample), "state vector doesn't match StateSample size");
|
||||
static_assert(sizeof(matrix::Vector<float, 25>) == sizeof(StateSample), "state vector doesn't match StateSample size");
|
||||
|
||||
struct IdxDof { unsigned idx; unsigned dof; };
|
||||
namespace State {
|
||||
|
|
@ -49,7 +51,8 @@ namespace State {
|
|||
static constexpr IdxDof mag_I{15, 3};
|
||||
static constexpr IdxDof mag_B{18, 3};
|
||||
static constexpr IdxDof wind_vel{21, 2};
|
||||
static constexpr uint8_t size{23};
|
||||
static constexpr IdxDof terrain_vpos{23, 1};
|
||||
static constexpr uint8_t size{24};
|
||||
};
|
||||
}
|
||||
#endif // !EKF_STATE_H
|
||||
|
|
|
|||
|
|
@ -112,17 +112,8 @@ void Ekf::controlRangeHeightFusion()
|
|||
innov_gate,
|
||||
aid_src);
|
||||
|
||||
// update the bias estimator before updating the main filter but after
|
||||
// using its current state to compute the vertical position innovation
|
||||
if (measurement_valid && _range_sensor.isDataHealthy()) {
|
||||
bias_est.setMaxStateNoise(sqrtf(measurement_var));
|
||||
bias_est.setProcessNoiseSpectralDensity(_params.rng_hgt_bias_nsd);
|
||||
bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
|
||||
}
|
||||
|
||||
// determine if we should use height aiding
|
||||
const bool do_conditional_range_aid = (_params.rng_ctrl == RngCtrl::CONDITIONAL) && isConditionalRangeAidSuitable();
|
||||
const bool continuing_conditions_passing = ((_params.rng_ctrl == RngCtrl::ENABLED) || do_conditional_range_aid)
|
||||
const bool continuing_conditions_passing = (_params.rng_ctrl == RngCtrl::ENABLED);
|
||||
&& measurement_valid
|
||||
&& _range_sensor.isDataHealthy();
|
||||
|
||||
|
|
@ -142,8 +133,8 @@ void Ekf::controlRangeHeightFusion()
|
|||
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
|
||||
|
||||
_information_events.flags.reset_hgt_to_rng = true;
|
||||
resetVerticalPositionTo(-(measurement - bias_est.getBias()));
|
||||
bias_est.setBias(_state.pos(2) + measurement);
|
||||
resetVerticalPositionTo(-(measurement - _state.terrain_vpos));
|
||||
_state.terrain_vpos = _state.pos(2) + measurement;
|
||||
|
||||
// reset vertical velocity
|
||||
resetVerticalVelocityToZero();
|
||||
|
|
@ -200,47 +191,6 @@ void Ekf::controlRangeHeightFusion()
|
|||
}
|
||||
}
|
||||
|
||||
bool Ekf::isConditionalRangeAidSuitable()
|
||||
{
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
if (_control_status.flags.in_air
|
||||
&& _range_sensor.isHealthy()
|
||||
&& isTerrainEstimateValid()) {
|
||||
// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
|
||||
// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
|
||||
float range_hagl_max = _params.max_hagl_for_range_aid;
|
||||
float max_vel_xy = _params.max_vel_for_range_aid;
|
||||
|
||||
const float hagl_innov = _aid_src_terrain_range_finder.innovation;
|
||||
const float hagl_innov_var = _aid_src_terrain_range_finder.innovation_variance;
|
||||
|
||||
const float hagl_test_ratio = (hagl_innov * hagl_innov / (sq(_params.range_aid_innov_gate) * hagl_innov_var));
|
||||
|
||||
bool is_hagl_stable = (hagl_test_ratio < 1.f);
|
||||
|
||||
if (!_control_status.flags.rng_hgt) {
|
||||
range_hagl_max = 0.7f * _params.max_hagl_for_range_aid;
|
||||
max_vel_xy = 0.7f * _params.max_vel_for_range_aid;
|
||||
is_hagl_stable = (hagl_test_ratio < 0.01f);
|
||||
}
|
||||
|
||||
const float range_hagl = _terrain_vpos - _state.pos(2);
|
||||
|
||||
const bool is_in_range = (range_hagl < range_hagl_max);
|
||||
|
||||
bool is_below_max_speed = true;
|
||||
|
||||
if (isHorizontalAidingActive()) {
|
||||
is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy);
|
||||
}
|
||||
|
||||
return is_in_range && is_hagl_stable && is_below_max_speed;
|
||||
}
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void Ekf::stopRngHgtFusion()
|
||||
{
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
|
|
|
|||
|
|
@ -45,11 +45,6 @@
|
|||
|
||||
void Ekf::initHagl()
|
||||
{
|
||||
// assume a ground clearance
|
||||
_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
|
||||
|
||||
// use the ground clearance value as our uncertainty
|
||||
_terrain_var = sq(_params.rng_gnd_clearance);
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_aid_src_terrain_range_finder.time_last_fuse = _time_delayed_us;
|
||||
|
|
@ -73,38 +68,6 @@ void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
|
|||
// as an overestimated distance leads to an overestimated velocity, causing a dangerous behavior.
|
||||
initHagl();
|
||||
}
|
||||
|
||||
predictHagl(imu_delayed);
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
controlHaglRngFusion();
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
controlHaglFlowFusion();
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
controlHaglFakeFusion();
|
||||
|
||||
// constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
|
||||
if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) {
|
||||
_terrain_vpos = _params.rng_gnd_clearance + _state.pos(2);
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::predictHagl(const imuSample &imu_delayed)
|
||||
{
|
||||
// predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle
|
||||
|
||||
// process noise due to errors in vehicle height estimate
|
||||
_terrain_var += sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise);
|
||||
|
||||
// process noise due to terrain gradient
|
||||
_terrain_var += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient)
|
||||
* (sq(_state.vel(0)) + sq(_state.vel(1)));
|
||||
|
||||
// limit the variance to prevent it becoming badly conditioned
|
||||
_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
|
|
@ -183,13 +146,6 @@ void Ekf::controlHaglRngFusion()
|
|||
}
|
||||
}
|
||||
|
||||
float Ekf::getRngVar() const
|
||||
{
|
||||
return fmaxf(P(State::pos.idx + 2, State::pos.idx + 2) * _params.vehicle_variance_scaler, 0.0f)
|
||||
+ sq(_params.range_noise)
|
||||
+ sq(_params.range_noise_scaler * _range_sensor.getRange());
|
||||
}
|
||||
|
||||
void Ekf::resetHaglRng()
|
||||
{
|
||||
_terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom();
|
||||
|
|
@ -343,81 +299,6 @@ void Ekf::resetHaglFlow()
|
|||
_aid_src_terrain_optical_flow.time_last_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow)
|
||||
{
|
||||
flow.fused = true;
|
||||
|
||||
const float R_LOS = flow.observation_variance[0];
|
||||
|
||||
// calculate the height above the ground of the optical flow camera. Since earth frame is NED
|
||||
// a positive offset in earth frame leads to a smaller height above the ground.
|
||||
float range = predictFlowRange();
|
||||
|
||||
const float state = _terrain_vpos; // linearize both axes using the same state value
|
||||
Vector2f innov_var;
|
||||
float H;
|
||||
sym::TerrEstComputeFlowXyInnovVarAndHx(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &innov_var, &H);
|
||||
innov_var.copyTo(flow.innovation_variance);
|
||||
|
||||
if ((flow.innovation_variance[0] < R_LOS)
|
||||
|| (flow.innovation_variance[1] < R_LOS)) {
|
||||
// we need to reinitialise the covariance matrix and abort this fusion step
|
||||
ECL_ERR("Opt flow error - covariance reset");
|
||||
_terrain_var = 100.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
// run the innovation consistency check and record result
|
||||
setEstimatorAidStatusTestRatio(flow, math::max(_params.flow_innov_gate, 1.f));
|
||||
|
||||
_innov_check_fail_status.flags.reject_optflow_X = (flow.test_ratio[0] > 1.f);
|
||||
_innov_check_fail_status.flags.reject_optflow_Y = (flow.test_ratio[1] > 1.f);
|
||||
|
||||
// if either axis fails we abort the fusion
|
||||
if (flow.innovation_rejected) {
|
||||
return;
|
||||
}
|
||||
|
||||
// fuse observation axes sequentially
|
||||
for (uint8_t index = 0; index <= 1; index++) {
|
||||
if (index == 0) {
|
||||
// everything was already computed above
|
||||
|
||||
} else if (index == 1) {
|
||||
// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
|
||||
sym::TerrEstComputeFlowYInnovVarAndH(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &flow.innovation_variance[1], &H);
|
||||
|
||||
// recalculate the innovation using the updated state
|
||||
const Vector2f vel_body = predictFlowVelBody();
|
||||
range = predictFlowRange();
|
||||
flow.innovation[1] = (-vel_body(0) / range) - flow.observation[1];
|
||||
|
||||
if (flow.innovation_variance[1] < R_LOS) {
|
||||
// we need to reinitialise the covariance matrix and abort this fusion step
|
||||
ECL_ERR("Opt flow error - covariance reset");
|
||||
_terrain_var = 100.0f;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
float Kfusion = _terrain_var * H / flow.innovation_variance[index];
|
||||
|
||||
_terrain_vpos += Kfusion * flow.innovation[0];
|
||||
// constrain terrain to minimum allowed value and predict height above ground
|
||||
_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
|
||||
|
||||
// guard against negative variance
|
||||
_terrain_var = fmaxf(_terrain_var - Kfusion * H * _terrain_var, sq(0.01f));
|
||||
}
|
||||
|
||||
_fault_status.flags.bad_optflow_X = false;
|
||||
_fault_status.flags.bad_optflow_Y = false;
|
||||
|
||||
flow.time_last_fuse = _time_delayed_us;
|
||||
flow.fused = true;
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
void Ekf::controlHaglFakeFusion()
|
||||
{
|
||||
if (!_control_status.flags.in_air
|
||||
|
|
@ -467,7 +348,3 @@ bool Ekf::isTerrainEstimateValid() const
|
|||
|
||||
return valid;
|
||||
}
|
||||
|
||||
void Ekf::terrainHandleVerticalPositionReset(const float delta_z) {
|
||||
_terrain_vpos += delta_z;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue