mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-25 15:03:33 +08:00
ekf2: add terrain state
This commit is contained in:
bresch
@@ -125,6 +125,7 @@ if(CONFIG_EKF2_RANGE_FINDER)
|
||||
list(APPEND EKF_SRCS
|
||||
aid_sources/range_finder/range_finder_consistency_check.cpp
|
||||
aid_sources/range_finder/range_height_control.cpp
|
||||
aid_sources/range_finder/range_height_fusion.cpp
|
||||
aid_sources/range_finder/sensor_range_finder.cpp
|
||||
)
|
||||
endif()
|
||||
|
||||
@@ -55,13 +55,9 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
|
||||
&& !_flow_sample_delayed.flow_rate.longerThan(_flow_max_rate);
|
||||
const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
|
||||
|
||||
// don't enforce this condition if terrain estimate is not valid as we have logic below to coast through bad range finder data
|
||||
const bool is_within_max_sensor_dist = isTerrainEstimateValid() ? (_terrain_vpos - _state.pos(2) <= _flow_max_distance) : true;
|
||||
|
||||
if (is_quality_good
|
||||
&& is_magnitude_good
|
||||
&& is_tilt_good
|
||||
&& is_within_max_sensor_dist) {
|
||||
&& is_tilt_good) {
|
||||
// compensate for body motion to give a LOS rate
|
||||
calcOptFlowBodyRateComp(imu_delayed);
|
||||
_flow_rate_compensated = _flow_sample_delayed.flow_rate - _flow_sample_delayed.gyro_rate.xy();
|
||||
@@ -81,20 +77,21 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
|
||||
&& (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
|
||||
|| isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow));
|
||||
|
||||
// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
|
||||
// use a relaxed time criteria to enable it to coast through bad range finder data
|
||||
const bool terrain_available = isTerrainEstimateValid() || isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)10e6);
|
||||
const bool is_within_max_sensor_dist = getHagl() <= _flow_max_distance;
|
||||
|
||||
const bool continuing_conditions_passing = (_params.flow_ctrl == 1)
|
||||
&& _control_status.flags.tilt_align
|
||||
&& (terrain_available || is_flow_required);
|
||||
&& is_within_max_sensor_dist;
|
||||
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
&& isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching
|
||||
|
||||
// If the height is relative to the ground, terrain height cannot be observed.
|
||||
_hagl_sensor_status.flags.flow = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE);
|
||||
|
||||
if (_control_status.flags.opt_flow) {
|
||||
if (continuing_conditions_passing) {
|
||||
fuseOptFlow();
|
||||
fuseOptFlow(_hagl_sensor_status.flags.flow);
|
||||
|
||||
// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
|
||||
if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)) {
|
||||
@@ -125,19 +122,35 @@ void Ekf::startFlowFusion()
|
||||
{
|
||||
ECL_INFO("starting optical flow fusion");
|
||||
|
||||
if (!_aid_src_optical_flow.innovation_rejected && isHorizontalAidingActive()) {
|
||||
// Consistent with the current velocity state, simply fuse the data without reset
|
||||
fuseOptFlow();
|
||||
_control_status.flags.opt_flow = true;
|
||||
if (_height_sensor_ref != HeightSensor::RANGE) {
|
||||
// If the height is relative to the ground, terrain height cannot be observed.
|
||||
_hagl_sensor_status.flags.flow = true;
|
||||
}
|
||||
|
||||
} else if (!isHorizontalAidingActive()) {
|
||||
resetFlowFusion();
|
||||
_control_status.flags.opt_flow = true;
|
||||
if (isHorizontalAidingActive()) {
|
||||
if (!_aid_src_optical_flow.innovation_rejected) {
|
||||
ECL_INFO("starting optical flow no reset");
|
||||
fuseOptFlow(_hagl_sensor_status.flags.flow);
|
||||
|
||||
} else if (_hagl_sensor_status.flags.flow && !_hagl_sensor_status.flags.range_finder) {
|
||||
resetHaglFlow();
|
||||
|
||||
} else {
|
||||
ECL_INFO("optical flow fusion failed to start");
|
||||
_control_status.flags.opt_flow = false;
|
||||
_hagl_sensor_status.flags.flow = false;
|
||||
}
|
||||
|
||||
} else {
|
||||
ECL_INFO("optical flow fusion failed to start");
|
||||
_control_status.flags.opt_flow = false;
|
||||
if (isTerrainEstimateValid() || (_height_sensor_ref == HeightSensor::RANGE)) {
|
||||
resetFlowFusion();
|
||||
|
||||
} else if (_hagl_sensor_status.flags.flow) {
|
||||
resetHaglFlow();
|
||||
}
|
||||
}
|
||||
|
||||
_control_status.flags.opt_flow = true; // needs to be here because of isHorizontalAidingActive
|
||||
}
|
||||
|
||||
void Ekf::resetFlowFusion()
|
||||
@@ -159,11 +172,26 @@ void Ekf::resetFlowFusion()
|
||||
_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::resetHaglFlow()
|
||||
{
|
||||
ECL_INFO("reset hagl to flow");
|
||||
// TODO: use the flow data
|
||||
_state.terrain = fmaxf(0.0f, _state.pos(2));
|
||||
P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 100.f);
|
||||
_terrain_vpos_reset_counter++;
|
||||
|
||||
_innov_check_fail_status.flags.reject_optflow_X = false;
|
||||
_innov_check_fail_status.flags.reject_optflow_Y = false;
|
||||
|
||||
_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::stopFlowFusion()
|
||||
{
|
||||
if (_control_status.flags.opt_flow) {
|
||||
ECL_INFO("stopping optical flow fusion");
|
||||
_control_status.flags.opt_flow = false;
|
||||
_hagl_sensor_status.flags.flow = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -73,7 +73,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);
|
||||
|
||||
// run the innovation consistency check and record result
|
||||
updateAidSourceStatus(aid_src,
|
||||
@@ -85,7 +85,7 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
|
||||
math::max(_params.flow_innov_gate, 1.f)); // innovation gate
|
||||
}
|
||||
|
||||
void Ekf::fuseOptFlow()
|
||||
void Ekf::fuseOptFlow(const bool update_terrain)
|
||||
{
|
||||
const float R_LOS = _aid_src_optical_flow.observation_variance[0];
|
||||
|
||||
@@ -97,7 +97,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 ((innov_var(0) < R_LOS) || (innov_var(1) < R_LOS)) {
|
||||
@@ -124,7 +124,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();
|
||||
@@ -141,6 +141,10 @@ void Ekf::fuseOptFlow()
|
||||
|
||||
VectorState Kfusion = P * H / _aid_src_optical_flow.innovation_variance[index];
|
||||
|
||||
if (!update_terrain) {
|
||||
Kfusion(State::terrain.idx) = 0.f;
|
||||
}
|
||||
|
||||
if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index], _aid_src_optical_flow.innovation[index])) {
|
||||
fused[index] = true;
|
||||
}
|
||||
@@ -165,10 +169,17 @@ 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 = getHagl() - pos_offset_earth(2);
|
||||
|
||||
// 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
|
||||
float flow_range = height_above_gnd_est / _R_to_earth(2, 2); // absolute distance to the frame region in view
|
||||
|
||||
// avoid the flow prediction singularity at range = 0
|
||||
if (fabsf(flow_range) < FLT_EPSILON) {
|
||||
flow_range = signNoZero(flow_range) * FLT_EPSILON;
|
||||
}
|
||||
|
||||
return flow_range;
|
||||
}
|
||||
|
||||
Vector2f Ekf::predictFlowVelBody()
|
||||
|
||||
@@ -37,6 +37,7 @@
|
||||
*/
|
||||
|
||||
#include "ekf.h"
|
||||
#include "ekf_derivation/generated/compute_hagl_innov_var.h"
|
||||
|
||||
void Ekf::controlRangeHeightFusion()
|
||||
{
|
||||
@@ -93,58 +94,91 @@ void Ekf::controlRangeHeightFusion()
|
||||
}
|
||||
|
||||
auto &aid_src = _aid_src_rng_hgt;
|
||||
HeightBiasEstimator &bias_est = _rng_hgt_b_est;
|
||||
|
||||
bias_est.predict(_dt_ekf_avg);
|
||||
|
||||
if (rng_data_ready && _range_sensor.getSampleAddress()) {
|
||||
|
||||
const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
|
||||
const float measurement_var = sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
|
||||
updateRangeHeight(aid_src);
|
||||
const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
|
||||
|
||||
const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
|
||||
|
||||
// vertical position innovation - baro measurement has opposite sign to earth z axis
|
||||
updateVerticalPositionAidStatus(aid_src,
|
||||
_range_sensor.getSampleAddress()->time_us, // sample timestamp
|
||||
-(measurement - bias_est.getBias()), // observation
|
||||
measurement_var + bias_est.getBiasVar(), // observation variance
|
||||
math::max(_params.range_innov_gate, 1.f)); // innovation gate
|
||||
|
||||
// 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 == static_cast<int32_t>(RngCtrl::CONDITIONAL))
|
||||
&& isConditionalRangeAidSuitable();
|
||||
|
||||
const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED)) || do_conditional_range_aid)
|
||||
&& measurement_valid
|
||||
&& _range_sensor.isDataHealthy();
|
||||
const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED))
|
||||
|| (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)))
|
||||
&& _control_status.flags.tilt_align
|
||||
&& measurement_valid
|
||||
&& _range_sensor.isDataHealthy()
|
||||
&& _rng_consistency_check.isKinematicallyConsistent();
|
||||
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
|
||||
&& _range_sensor.isRegularlySendingData();
|
||||
|
||||
|
||||
const bool do_conditional_range_aid = (_hagl_sensor_status.flags.range_finder || _control_status.flags.rng_hgt)
|
||||
&& (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
|
||||
&& isConditionalRangeAidSuitable();
|
||||
|
||||
const bool do_range_aid = (_hagl_sensor_status.flags.range_finder || _control_status.flags.rng_hgt)
|
||||
&& (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED));
|
||||
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
if (!(do_conditional_range_aid || do_range_aid)) {
|
||||
ECL_INFO("stopping %s fusion", HGT_SRC_NAME);
|
||||
stopRngHgtFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
|
||||
if (do_conditional_range_aid) {
|
||||
// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
|
||||
ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
|
||||
_height_sensor_ref = HeightSensor::RANGE;
|
||||
|
||||
_control_status.flags.rng_hgt = true;
|
||||
stopRngTerrFusion();
|
||||
|
||||
if (!_hagl_sensor_status.flags.flow && aid_src.innovation_rejected) {
|
||||
resetHaglRng(aid_src);
|
||||
}
|
||||
|
||||
} else if (do_range_aid) {
|
||||
// Range finder is the primary height source, the ground is now the datum used
|
||||
// to compute the local vertical position
|
||||
ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
|
||||
_height_sensor_ref = HeightSensor::RANGE;
|
||||
|
||||
_information_events.flags.reset_hgt_to_rng = true;
|
||||
resetVerticalPositionTo(-aid_src.observation, aid_src.observation_variance);
|
||||
_state.terrain = 0.f;
|
||||
_control_status.flags.rng_hgt = true;
|
||||
stopRngTerrFusion();
|
||||
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (do_conditional_range_aid || do_range_aid) {
|
||||
ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
|
||||
_control_status.flags.rng_hgt = true;
|
||||
|
||||
if (!_hagl_sensor_status.flags.flow && aid_src.innovation_rejected) {
|
||||
resetHaglRng(aid_src);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_control_status.flags.rng_hgt || _hagl_sensor_status.flags.range_finder) {
|
||||
if (continuing_conditions_passing) {
|
||||
|
||||
fuseVerticalPosition(aid_src);
|
||||
fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _hagl_sensor_status.flags.range_finder);
|
||||
|
||||
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
|
||||
|
||||
if (isHeightResetRequired()) {
|
||||
if (isHeightResetRequired() && _control_status.flags.rng_hgt) {
|
||||
// All height sources are failing
|
||||
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(-(aid_src.observation - _state.terrain));
|
||||
|
||||
// reset vertical velocity
|
||||
resetVerticalVelocityToZero();
|
||||
@@ -153,96 +187,123 @@ void Ekf::controlRangeHeightFusion()
|
||||
|
||||
} else if (is_fusion_failing) {
|
||||
// Some other height source is still working
|
||||
ECL_WARN("stopping %s height fusion, fusion failing", HGT_SRC_NAME);
|
||||
stopRngHgtFusion();
|
||||
_control_status.flags.rng_fault = true;
|
||||
_range_sensor.setFaulty();
|
||||
if (_hagl_sensor_status.flags.flow) {
|
||||
ECL_WARN("stopping %s fusion, fusion failing", HGT_SRC_NAME);
|
||||
stopRngHgtFusion();
|
||||
stopRngTerrFusion();
|
||||
|
||||
} else {
|
||||
resetHaglRng(aid_src);
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
ECL_WARN("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME);
|
||||
ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
|
||||
stopRngHgtFusion();
|
||||
stopRngTerrFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE))
|
||||
&& (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
|
||||
) {
|
||||
// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
|
||||
ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
|
||||
_height_sensor_ref = HeightSensor::RANGE;
|
||||
bias_est.setBias(_state.pos(2) + measurement);
|
||||
|
||||
} else if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE))
|
||||
&& (_params.rng_ctrl != static_cast<int32_t>(RngCtrl::CONDITIONAL))
|
||||
) {
|
||||
// Range finder is the primary height source, the ground is now the datum used
|
||||
// to compute the local vertical position
|
||||
ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
|
||||
_height_sensor_ref = HeightSensor::RANGE;
|
||||
|
||||
_information_events.flags.reset_hgt_to_rng = true;
|
||||
resetVerticalPositionTo(-measurement, measurement_var);
|
||||
bias_est.reset();
|
||||
if (_hagl_sensor_status.flags.flow) {
|
||||
if (!aid_src.innovation_rejected) {
|
||||
_hagl_sensor_status.flags.range_finder = true;
|
||||
fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _hagl_sensor_status.flags.range_finder);
|
||||
}
|
||||
|
||||
} else {
|
||||
ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
|
||||
bias_est.setBias(_state.pos(2) + measurement);
|
||||
}
|
||||
if (aid_src.innovation_rejected) {
|
||||
resetHaglRng(aid_src);
|
||||
}
|
||||
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
bias_est.setFusionActive();
|
||||
_control_status.flags.rng_hgt = true;
|
||||
_hagl_sensor_status.flags.range_finder = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.rng_hgt
|
||||
} else if ((_control_status.flags.rng_hgt || _hagl_sensor_status.flags.range_finder)
|
||||
&& !isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)) {
|
||||
// No data anymore. Stop until it comes back.
|
||||
ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME);
|
||||
ECL_WARN("stopping %s fusion, no data", HGT_SRC_NAME);
|
||||
stopRngHgtFusion();
|
||||
stopRngTerrFusion();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::updateRangeHeight(estimator_aid_source1d_s &aid_src)
|
||||
{
|
||||
aid_src.observation = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
|
||||
aid_src.innovation = getHagl() - aid_src.observation;
|
||||
|
||||
const float observation_variance = getRngVar();
|
||||
float innovation_variance;
|
||||
sym::ComputeHaglInnovVar(P, observation_variance, &innovation_variance);
|
||||
|
||||
const float innov_gate = math::max(_params.range_innov_gate, 1.f);
|
||||
updateAidSourceStatus(aid_src,
|
||||
_range_sensor.getSampleAddress()->time_us, // sample timestamp
|
||||
math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance), // observation
|
||||
observation_variance, // observation variance
|
||||
getHagl() - aid_src.observation, // innovation
|
||||
innovation_variance, // innovation variance
|
||||
math::max(_params.range_innov_gate, 1.f)); // innovation gate
|
||||
|
||||
// z special case if there is bad vertical acceleration data, then don't reject measurement,
|
||||
// but limit innovation to prevent spikes that could destabilise the filter
|
||||
if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
|
||||
const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
|
||||
aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
|
||||
aid_src.innovation_rejected = false;
|
||||
}
|
||||
}
|
||||
|
||||
float Ekf::getRngVar() const
|
||||
{
|
||||
return fmaxf(
|
||||
P(State::pos.idx + 2, State::pos.idx + 2)
|
||||
+ sq(_params.range_noise)
|
||||
+ sq(_params.range_noise_scaler * _range_sensor.getRange()),
|
||||
0.f);
|
||||
}
|
||||
|
||||
void Ekf::resetHaglRng(estimator_aid_source1d_s &aid_src)
|
||||
{
|
||||
_state.terrain = _state.pos(2) + aid_src.observation;
|
||||
P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, aid_src.observation_variance);
|
||||
_terrain_vpos_reset_counter++;
|
||||
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
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;
|
||||
// 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 = _aid_src_rng_hgt.test_ratio;
|
||||
|
||||
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);
|
||||
|
||||
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;
|
||||
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 bool is_in_range = (getHagl() < 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;
|
||||
@@ -256,8 +317,11 @@ void Ekf::stopRngHgtFusion()
|
||||
_height_sensor_ref = HeightSensor::UNKNOWN;
|
||||
}
|
||||
|
||||
_rng_hgt_b_est.setFusionInactive();
|
||||
|
||||
_control_status.flags.rng_hgt = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::stopRngTerrFusion()
|
||||
{
|
||||
_hagl_sensor_status.flags.range_finder = false;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,70 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "ekf.h"
|
||||
#include "ekf_derivation/generated/compute_hagl_h.h"
|
||||
|
||||
bool Ekf::fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain)
|
||||
{
|
||||
if (aid_src.innovation_rejected) {
|
||||
_innov_check_fail_status.flags.reject_hagl = true;
|
||||
return false;
|
||||
}
|
||||
|
||||
VectorState H;
|
||||
|
||||
sym::ComputeHaglH(&H);
|
||||
|
||||
// calculate the Kalman gain
|
||||
VectorState K = P * H / aid_src.innovation_variance;
|
||||
|
||||
if (!update_terrain) {
|
||||
K(State::terrain.idx) = 0.f;
|
||||
}
|
||||
|
||||
if (!update_height) {
|
||||
const float k_terrain = K(State::terrain.idx);
|
||||
K.zero();
|
||||
K(State::terrain.idx) = k_terrain;
|
||||
}
|
||||
|
||||
measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation);
|
||||
|
||||
// record last successful fusion event
|
||||
_innov_check_fail_status.flags.reject_hagl = false;
|
||||
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
aid_src.fused = true;
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -107,7 +107,7 @@ enum MagFuseType : uint8_t {
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
enum TerrainFusionMask : uint8_t {
|
||||
enum class TerrainFusionMask : uint8_t {
|
||||
TerrainFuseRangeFinder = (1 << 0),
|
||||
TerrainFuseOpticalFlow = (1 << 1)
|
||||
};
|
||||
@@ -382,8 +382,8 @@ struct parameters {
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
|
||||
TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
|
||||
int32_t terrain_fusion_mode{static_cast<int32_t>(TerrainFusionMask::TerrainFuseRangeFinder)
|
||||
| static_cast<int32_t>(TerrainFusionMask::TerrainFuseOpticalFlow)}; ///< aiding source(s) selection bitmask for the terrain estimation
|
||||
|
||||
float terrain_p_noise{5.0f}; ///< process noise for terrain offset (m/sec)
|
||||
float terrain_gradient{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m)
|
||||
@@ -401,10 +401,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
|
||||
@@ -521,7 +519,7 @@ union innovation_fault_status_u {
|
||||
bool reject_yaw : 1; ///< 7 - true if the yaw observation has been rejected
|
||||
bool reject_airspeed : 1; ///< 8 - true if the airspeed observation has been rejected
|
||||
bool reject_sideslip : 1; ///< 9 - true if the synthetic sideslip observation has been rejected
|
||||
bool reject_hagl : 1; ///< 10 - true if the height above ground observation has been rejected
|
||||
bool reject_hagl : 1; ///< 10 - unused
|
||||
bool reject_optflow_X : 1; ///< 11 - true if the X optical flow observation has been rejected
|
||||
bool reject_optflow_Y : 1; ///< 12 - true if the Y optical flow observation has been rejected
|
||||
} flags;
|
||||
|
||||
@@ -202,6 +202,17 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
|
||||
}
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
if (_height_sensor_ref != HeightSensor::RANGE) {
|
||||
// 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
|
||||
float terrain_process_noise = sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise);
|
||||
|
||||
// process noise due to terrain gradient
|
||||
terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(1)));
|
||||
P(State::terrain.idx, State::terrain.idx) += terrain_process_noise;
|
||||
}
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
// covariance matrix is symmetrical, so copy upper half to lower half
|
||||
for (unsigned row = 0; row < State::size; row++) {
|
||||
@@ -239,6 +250,10 @@ void Ekf::constrainStateVariances()
|
||||
constrainStateVarLimitRatio(State::wind_vel, 1e-6f, 1e6f);
|
||||
}
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
constrainStateVarLimitRatio(State::terrain, 0.f, 1e4f);
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
}
|
||||
|
||||
void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
|
||||
|
||||
@@ -161,7 +161,6 @@ bool Ekf::update()
|
||||
controlFusionModes(imu_sample_delayed);
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
// run a separate filter for terrain estimation
|
||||
runTerrainEstimator(imu_sample_delayed);
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
@@ -201,7 +200,7 @@ bool Ekf::initialiseFilter()
|
||||
initialiseCovariance();
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
// Initialise the terrain estimator
|
||||
// Initialise the terrain state
|
||||
initHagl();
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
@@ -377,6 +376,14 @@ void Ekf::print_status()
|
||||
);
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
printf("Terrain position (%d): %.3f var: %.1e\n",
|
||||
State::terrain.idx,
|
||||
(double)_state.terrain,
|
||||
(double)getStateVariance<State::terrain>()(0)
|
||||
);
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
printf("\nP:\n");
|
||||
P.print();
|
||||
|
||||
|
||||
@@ -104,27 +104,19 @@ 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; };
|
||||
float getHagl() const { return _state.terrain - _state.pos(2); }
|
||||
|
||||
// get the number of times the vertical terrain position has been reset
|
||||
uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; };
|
||||
|
||||
// get the terrain variance
|
||||
float get_terrain_var() const { return _terrain_var; }
|
||||
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
const auto &aid_src_terrain_range_finder() const { return _aid_src_terrain_range_finder; }
|
||||
# endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
float getTerrainVariance() const { return P(State::terrain.idx, State::terrain.idx); }
|
||||
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// range height
|
||||
const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
|
||||
const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
|
||||
|
||||
float getHaglRateInnov() const { return _rng_consistency_check.getInnov(); }
|
||||
@@ -592,27 +584,14 @@ private:
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
// Terrain height state estimation
|
||||
float _terrain_vpos{0.0f}; ///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
|
||||
float _terrain_var{1e4f}; ///< variance of terrain position estimate (m**2)
|
||||
uint8_t _terrain_vpos_reset_counter{0}; ///< number of times _terrain_vpos has been reset
|
||||
|
||||
terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
|
||||
float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m)
|
||||
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
estimator_aid_source1d_s _aid_src_terrain_range_finder{};
|
||||
uint64_t _time_last_healthy_rng_data{0};
|
||||
# endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
estimator_aid_source2d_s _aid_src_terrain_optical_flow{};
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
estimator_aid_source1d_s _aid_src_rng_hgt{};
|
||||
HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
@@ -831,29 +810,22 @@ private:
|
||||
// terrain vertical position estimator
|
||||
void initHagl();
|
||||
void runTerrainEstimator(const imuSample &imu_delayed);
|
||||
void predictHagl(const imuSample &imu_delayed);
|
||||
|
||||
float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
|
||||
float getTerrainVPos() const { return isTerrainEstimateValid() ? _state.terrain : _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
|
||||
void controlHaglRngFusion();
|
||||
void updateHaglRng(estimator_aid_source1d_s &aid_src) const;
|
||||
void fuseHaglRng(estimator_aid_source1d_s &aid_src);
|
||||
void resetHaglRng();
|
||||
void stopHaglRngFusion();
|
||||
bool fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain);
|
||||
void updateRangeHeight(estimator_aid_source1d_s &aid_src);
|
||||
void resetHaglRng(estimator_aid_source1d_s &aid_src);
|
||||
float getRngVar() const;
|
||||
# endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
// update the terrain vertical position estimate using an optical flow measurement
|
||||
void controlHaglFlowFusion();
|
||||
void resetHaglFlow();
|
||||
void stopHaglFlowFusion();
|
||||
void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
@@ -863,6 +835,7 @@ private:
|
||||
void controlRangeHeightFusion();
|
||||
bool isConditionalRangeAidSuitable();
|
||||
void stopRngHgtFusion();
|
||||
void stopRngTerrFusion();
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
@@ -883,7 +856,7 @@ private:
|
||||
|
||||
// fuse optical flow line of sight rate measurements
|
||||
void updateOptFlow(estimator_aid_source2d_s &aid_src);
|
||||
void fuseOptFlow();
|
||||
void fuseOptFlow(bool update_terrain);
|
||||
float predictFlowRange();
|
||||
Vector2f predictFlowVelBody();
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
@@ -211,7 +211,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
if (_control_status.flags.opt_flow) {
|
||||
float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
|
||||
vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm();
|
||||
vel_err_conservative = math::max(getHagl(), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm();
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
@@ -271,7 +271,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
|
||||
const float flow_hagl_min = fmaxf(rangefinder_hagl_min, _flow_min_distance);
|
||||
const float flow_hagl_max = fminf(rangefinder_hagl_max, _flow_max_distance);
|
||||
|
||||
const float flow_constrained_height = math::constrain(_terrain_vpos - _state.pos(2), flow_hagl_min, flow_hagl_max);
|
||||
const float flow_constrained_height = math::constrain(getHagl(), flow_hagl_min, flow_hagl_max);
|
||||
|
||||
// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
|
||||
const float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height;
|
||||
@@ -472,18 +472,9 @@ float Ekf::getHeightAboveGroundInnovationTestRatio() const
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
if (_hagl_sensor_status.flags.range_finder) {
|
||||
// return the terrain height innovation test ratio
|
||||
test_ratio = math::max(test_ratio, fabsf(_aid_src_terrain_range_finder.test_ratio_filtered));
|
||||
test_ratio = math::max(test_ratio, fabsf(_aid_src_rng_hgt.test_ratio_filtered));
|
||||
}
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
if (_hagl_sensor_status.flags.flow) {
|
||||
// return the terrain height innovation test ratio
|
||||
for (auto &test_ratio_filtered : _aid_src_terrain_optical_flow.test_ratio_filtered) {
|
||||
test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered));
|
||||
}
|
||||
}
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
# endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) {
|
||||
@@ -569,6 +560,10 @@ void Ekf::fuse(const VectorState &K, float innovation)
|
||||
_state.wind_vel = matrix::constrain(_state.wind_vel - K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) * innovation, -1.e2f, 1.e2f);
|
||||
}
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
_state.terrain = math::constrain(_state.terrain - K(State::terrain.idx) * innovation, -1e4f, 1e4f);
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
}
|
||||
|
||||
void Ekf::updateDeadReckoningStatus()
|
||||
@@ -670,7 +665,7 @@ void Ekf::updateGroundEffect()
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
if (isTerrainEstimateValid()) {
|
||||
// automatically set ground effect if terrain is valid
|
||||
float height = _terrain_vpos - _state.pos(2);
|
||||
float height = getHagl();
|
||||
_control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt);
|
||||
|
||||
} else
|
||||
|
||||
@@ -301,7 +301,8 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
checks[3] = {ReferenceType::GROUND, _aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance};
|
||||
// Range is a distance to ground measurement, not a direct height observation and has an opposite sign
|
||||
checks[3] = {ReferenceType::GROUND, -_aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance};
|
||||
}
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
|
||||
@@ -161,12 +161,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);
|
||||
_state.terrain += delta_z;
|
||||
#endif
|
||||
|
||||
// Reset the timout timer
|
||||
|
||||
@@ -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 = 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 = sf.V1.symbolic("terrain")
|
||||
)
|
||||
|
||||
if args.disable_mag:
|
||||
@@ -456,7 +458,10 @@ 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_hagl(state):
|
||||
return state["terrain"][0] - state["pos"][2]
|
||||
|
||||
def predict_opt_flow(state, epsilon):
|
||||
R_to_body = state["quat_nominal"].inverse()
|
||||
|
||||
# Calculate earth relative velocity in a non-rotating sensor frame
|
||||
@@ -464,23 +469,23 @@ def predict_opt_flow(state, distance, epsilon):
|
||||
|
||||
# Divide by range to get predicted angular LOS rates relative to X and Y
|
||||
# axes. Note these are rates in a non-rotating sensor frame
|
||||
hagl = predict_hagl(state)
|
||||
hagl = add_epsilon_sign(hagl, hagl, epsilon)
|
||||
R_to_earth = state["quat_nominal"].to_rotation_matrix()
|
||||
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] / hagl * R_to_earth[2, 2]
|
||||
flow_pred[1] = -rel_vel_sensor[0] / hagl * R_to_earth[2, 2]
|
||||
|
||||
return flow_pred
|
||||
|
||||
|
||||
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 = jacobian_chain_rule(meas_pred[0], state)
|
||||
@@ -493,18 +498,40 @@ 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 = jacobian_chain_rule(meas_pred[1], state)
|
||||
innov_var = (Hy * P * Hy.T + R)[0,0]
|
||||
|
||||
return (innov_var, Hy.T)
|
||||
|
||||
def compute_hagl_innov_var(
|
||||
P: MTangent,
|
||||
R: sf.Scalar,
|
||||
) -> (sf.Scalar):
|
||||
state = VState.symbolic("state")
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_hagl(state)
|
||||
|
||||
H = jacobian_chain_rule(meas_pred, state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var)
|
||||
|
||||
def compute_hagl_h(
|
||||
) -> (VTangent):
|
||||
state = VState.symbolic("state")
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_hagl(state)
|
||||
|
||||
H = jacobian_chain_rule(meas_pred, state)
|
||||
|
||||
return (H.T)
|
||||
|
||||
def compute_gnss_yaw_pred_innov_var_and_h(
|
||||
state: VState,
|
||||
P: MTangent,
|
||||
@@ -664,6 +691,8 @@ if not args.disable_wind:
|
||||
generate_px4_function(compute_yaw_innov_var_and_h, output_names=["innov_var", "H"])
|
||||
generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
|
||||
generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])
|
||||
generate_px4_function(compute_hagl_innov_var, output_names=["innov_var"])
|
||||
generate_px4_function(compute_hagl_h, output_names=["H"])
|
||||
generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "H"])
|
||||
generate_px4_function(compute_gravity_xyz_innov_var_and_hx, output_names=["innov_var", "Hx"])
|
||||
generate_px4_function(compute_gravity_y_innov_var_and_h, output_names=["innov_var", "Hy"])
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
+4
-4
@@ -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) {
|
||||
|
||||
+7
-7
@@ -16,8 +16,8 @@ namespace sym {
|
||||
* Symbolic function: compute_drag_x_innov_var_and_h
|
||||
*
|
||||
* 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
|
||||
* Hx: Matrix23_1
|
||||
* Hx: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeDragXInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar rho,
|
||||
void ComputeDragXInnovVarAndH(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 Hx = nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>* const Hx = nullptr) {
|
||||
// Total ops: 357
|
||||
|
||||
// Input arrays
|
||||
@@ -162,7 +162,7 @@ void ComputeDragXInnovVarAndH(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();
|
||||
|
||||
|
||||
+7
-7
@@ -16,8 +16,8 @@ namespace sym {
|
||||
* Symbolic function: compute_drag_y_innov_var_and_h
|
||||
*
|
||||
* 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
|
||||
* Hy: Matrix23_1
|
||||
* Hy: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeDragYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar rho,
|
||||
void ComputeDragYInnovVarAndH(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 Hy = nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>* const Hy = nullptr) {
|
||||
// Total ops: 360
|
||||
|
||||
// Input arrays
|
||||
@@ -162,7 +162,7 @@ void ComputeDragYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
}
|
||||
|
||||
if (Hy != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _hy = (*Hy);
|
||||
matrix::Matrix<Scalar, 24, 1>& _hy = (*Hy);
|
||||
|
||||
_hy.setZero();
|
||||
|
||||
|
||||
+125
-89
@@ -16,117 +16,153 @@ 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: 275
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 431
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (42)
|
||||
const Scalar _tmp0 = 2 * state(4, 0);
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 = _tmp1 * state(6, 0);
|
||||
const Scalar _tmp3 = -_tmp0 * state(3, 0) + _tmp2;
|
||||
const Scalar _tmp4 =
|
||||
Scalar(1.0) /
|
||||
(distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
|
||||
const Scalar _tmp5 = (Scalar(1) / Scalar(2)) * _tmp4;
|
||||
const Scalar _tmp6 = _tmp5 * state(2, 0);
|
||||
const Scalar _tmp7 = 2 * state(3, 0) * state(6, 0);
|
||||
const Scalar _tmp8 = _tmp5 * (_tmp0 * state(1, 0) + _tmp7);
|
||||
const Scalar _tmp9 = 2 * state(0, 0);
|
||||
const Scalar _tmp10 = state(3, 0) * state(5, 0);
|
||||
const Scalar _tmp11 = 2 * state(2, 0);
|
||||
const Scalar _tmp12 = _tmp11 * state(6, 0);
|
||||
const Scalar _tmp13 = -4 * _tmp10 + _tmp12 - _tmp9 * state(4, 0);
|
||||
const Scalar _tmp14 = _tmp5 * state(1, 0);
|
||||
const Scalar _tmp15 = _tmp9 * state(6, 0);
|
||||
const Scalar _tmp16 = _tmp0 * state(2, 0) + _tmp15 - 4 * state(1, 0) * state(5, 0);
|
||||
const Scalar _tmp17 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp18 = -_tmp13 * _tmp14 + _tmp16 * _tmp17 - _tmp3 * _tmp6 + _tmp8 * state(0, 0);
|
||||
const Scalar _tmp19 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp20 = _tmp4 * (_tmp19 - 2 * std::pow(state(1, 0), Scalar(2)));
|
||||
const Scalar _tmp21 = _tmp3 * _tmp5;
|
||||
const Scalar _tmp22 = _tmp5 * state(0, 0);
|
||||
const Scalar _tmp23 =
|
||||
_tmp13 * _tmp6 + _tmp16 * _tmp22 - _tmp21 * state(1, 0) - _tmp8 * state(3, 0);
|
||||
const Scalar _tmp24 =
|
||||
_tmp13 * _tmp22 - _tmp16 * _tmp6 - _tmp21 * state(3, 0) + _tmp8 * state(1, 0);
|
||||
const Scalar _tmp25 = _tmp9 * state(3, 0);
|
||||
const Scalar _tmp26 = _tmp1 * state(2, 0);
|
||||
const Scalar _tmp27 = _tmp4 * (-_tmp25 + _tmp26);
|
||||
const Scalar _tmp28 = _tmp4 * (_tmp11 * state(3, 0) + _tmp9 * state(1, 0));
|
||||
const Scalar _tmp29 = _tmp4 * (_tmp19 - 2 * std::pow(state(2, 0), Scalar(2)));
|
||||
const Scalar _tmp30 = 4 * state(4, 0);
|
||||
const Scalar _tmp31 = _tmp2 - _tmp30 * state(3, 0) + _tmp9 * state(5, 0);
|
||||
const Scalar _tmp32 = 2 * state(5, 0);
|
||||
const Scalar _tmp33 = -_tmp15 - _tmp30 * state(2, 0) + _tmp32 * state(1, 0);
|
||||
const Scalar _tmp34 = _tmp32 * state(2, 0) + _tmp7;
|
||||
const Scalar _tmp35 = 2 * _tmp10 - _tmp12;
|
||||
const Scalar _tmp36 = _tmp35 * _tmp5;
|
||||
const Scalar _tmp37 = _tmp17 * _tmp33 - _tmp22 * _tmp34 - _tmp31 * _tmp6 + _tmp36 * state(1, 0);
|
||||
const Scalar _tmp38 = -_tmp14 * _tmp33 - _tmp22 * _tmp31 + _tmp34 * _tmp6 + _tmp36 * state(3, 0);
|
||||
const Scalar _tmp39 = _tmp14 * _tmp31 - _tmp17 * _tmp34 - _tmp22 * _tmp33 + _tmp35 * _tmp6;
|
||||
const Scalar _tmp40 = _tmp4 * (_tmp25 + _tmp26);
|
||||
const Scalar _tmp41 = _tmp4 * (_tmp1 * state(3, 0) - _tmp9 * state(2, 0));
|
||||
// Intermediate terms (66)
|
||||
const Scalar _tmp0 = state(2, 0) * state(4, 0);
|
||||
const Scalar _tmp1 = state(1, 0) * state(5, 0);
|
||||
const Scalar _tmp2 = 2 * state(6, 0);
|
||||
const Scalar _tmp3 = _tmp2 * state(0, 0);
|
||||
const Scalar _tmp4 = -2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp5 = 1 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp6 = _tmp4 + _tmp5;
|
||||
const Scalar _tmp7 = state(24, 0) - state(9, 0);
|
||||
const Scalar _tmp8 =
|
||||
_tmp7 + epsilon * (2 * math::min<Scalar>(0, (((_tmp7) > 0) - ((_tmp7) < 0))) + 1);
|
||||
const Scalar _tmp9 = Scalar(1.0) / (_tmp8);
|
||||
const Scalar _tmp10 = _tmp6 * _tmp9;
|
||||
const Scalar _tmp11 = 2 * state(0, 0) * state(3, 0);
|
||||
const Scalar _tmp12 = 2 * state(1, 0);
|
||||
const Scalar _tmp13 = _tmp12 * state(2, 0);
|
||||
const Scalar _tmp14 = -_tmp11 + _tmp13;
|
||||
const Scalar _tmp15 = 2 * state(2, 0);
|
||||
const Scalar _tmp16 = _tmp12 * state(0, 0) + _tmp15 * state(3, 0);
|
||||
const Scalar _tmp17 = -2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp18 = _tmp17 + _tmp5;
|
||||
const Scalar _tmp19 = _tmp14 * state(4, 0) + _tmp16 * state(6, 0) + _tmp18 * state(5, 0);
|
||||
const Scalar _tmp20 = 4 * _tmp9;
|
||||
const Scalar _tmp21 = _tmp19 * _tmp20;
|
||||
const Scalar _tmp22 = _tmp10 * (2 * _tmp0 - 4 * _tmp1 + _tmp3) - _tmp21 * state(1, 0);
|
||||
const Scalar _tmp23 = (Scalar(1) / Scalar(2)) * _tmp22;
|
||||
const Scalar _tmp24 = 2 * state(4, 0);
|
||||
const Scalar _tmp25 = 4 * state(3, 0);
|
||||
const Scalar _tmp26 = _tmp2 * state(2, 0);
|
||||
const Scalar _tmp27 = -_tmp24 * state(0, 0) - _tmp25 * state(5, 0) + _tmp26;
|
||||
const Scalar _tmp28 = (Scalar(1) / Scalar(2)) * _tmp10;
|
||||
const Scalar _tmp29 = _tmp28 * state(0, 0);
|
||||
const Scalar _tmp30 = (Scalar(1) / Scalar(2)) * state(3, 0);
|
||||
const Scalar _tmp31 = _tmp2 * state(1, 0);
|
||||
const Scalar _tmp32 = _tmp10 * (-_tmp24 * state(3, 0) + _tmp31);
|
||||
const Scalar _tmp33 = _tmp2 * state(3, 0);
|
||||
const Scalar _tmp34 = _tmp10 * (_tmp24 * state(1, 0) + _tmp33) - _tmp21 * state(2, 0);
|
||||
const Scalar _tmp35 = (Scalar(1) / Scalar(2)) * _tmp34;
|
||||
const Scalar _tmp36 =
|
||||
-_tmp23 * state(2, 0) + _tmp27 * _tmp29 - _tmp30 * _tmp32 + _tmp35 * state(1, 0);
|
||||
const Scalar _tmp37 = _tmp10 * _tmp14;
|
||||
const Scalar _tmp38 = _tmp28 * state(2, 0);
|
||||
const Scalar _tmp39 = (Scalar(1) / Scalar(2)) * _tmp32;
|
||||
const Scalar _tmp40 =
|
||||
_tmp23 * state(0, 0) + _tmp27 * _tmp38 - _tmp30 * _tmp34 - _tmp39 * state(1, 0);
|
||||
const Scalar _tmp41 = _tmp28 * state(1, 0);
|
||||
const Scalar _tmp42 =
|
||||
_tmp22 * _tmp30 - _tmp27 * _tmp41 + _tmp35 * state(0, 0) - _tmp39 * state(2, 0);
|
||||
const Scalar _tmp43 = _tmp10 * _tmp18;
|
||||
const Scalar _tmp44 = _tmp6 / std::pow(_tmp8, Scalar(2));
|
||||
const Scalar _tmp45 = _tmp19 * _tmp44;
|
||||
const Scalar _tmp46 = _tmp10 * _tmp16;
|
||||
const Scalar _tmp47 = _tmp12 * state(3, 0) - _tmp15 * state(0, 0);
|
||||
const Scalar _tmp48 = _tmp10 * _tmp47;
|
||||
const Scalar _tmp49 = _tmp11 + _tmp13;
|
||||
const Scalar _tmp50 = _tmp10 * _tmp49;
|
||||
const Scalar _tmp51 = _tmp17 + _tmp4 + 1;
|
||||
const Scalar _tmp52 = _tmp10 * _tmp51;
|
||||
const Scalar _tmp53 = _tmp47 * state(6, 0) + _tmp49 * state(5, 0) + _tmp51 * state(4, 0);
|
||||
const Scalar _tmp54 = _tmp20 * _tmp53;
|
||||
const Scalar _tmp55 = 2 * state(5, 0);
|
||||
const Scalar _tmp56 = -_tmp10 * (_tmp33 + _tmp55 * state(2, 0)) + _tmp54 * state(1, 0);
|
||||
const Scalar _tmp57 = -_tmp10 * (-4 * _tmp0 + 2 * _tmp1 - _tmp3) + _tmp54 * state(2, 0);
|
||||
const Scalar _tmp58 = (Scalar(1) / Scalar(2)) * _tmp57;
|
||||
const Scalar _tmp59 = -_tmp25 * state(4, 0) + _tmp31 + _tmp55 * state(0, 0);
|
||||
const Scalar _tmp60 = -_tmp26 + _tmp55 * state(3, 0);
|
||||
const Scalar _tmp61 = _tmp30 * _tmp56 + _tmp38 * _tmp60 + _tmp41 * _tmp59 + _tmp58 * state(0, 0);
|
||||
const Scalar _tmp62 = (Scalar(1) / Scalar(2)) * _tmp56;
|
||||
const Scalar _tmp63 = -_tmp30 * _tmp57 - _tmp38 * _tmp59 + _tmp41 * _tmp60 + _tmp62 * state(0, 0);
|
||||
const Scalar _tmp64 =
|
||||
_tmp10 * _tmp30 * _tmp60 - _tmp29 * _tmp59 + _tmp58 * state(1, 0) - _tmp62 * state(2, 0);
|
||||
const Scalar _tmp65 = _tmp44 * _tmp53;
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
matrix::Matrix<Scalar, 2, 1>& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var(0, 0) = R +
|
||||
_tmp18 * (P(0, 1) * _tmp23 + P(1, 1) * _tmp18 + P(2, 1) * _tmp24 +
|
||||
P(3, 1) * _tmp27 + P(4, 1) * _tmp20 + P(5, 1) * _tmp28) +
|
||||
_tmp20 * (P(0, 4) * _tmp23 + P(1, 4) * _tmp18 + P(2, 4) * _tmp24 +
|
||||
P(3, 4) * _tmp27 + P(4, 4) * _tmp20 + P(5, 4) * _tmp28) +
|
||||
_tmp23 * (P(0, 0) * _tmp23 + P(1, 0) * _tmp18 + P(2, 0) * _tmp24 +
|
||||
P(3, 0) * _tmp27 + P(4, 0) * _tmp20 + P(5, 0) * _tmp28) +
|
||||
_tmp24 * (P(0, 2) * _tmp23 + P(1, 2) * _tmp18 + P(2, 2) * _tmp24 +
|
||||
P(3, 2) * _tmp27 + P(4, 2) * _tmp20 + P(5, 2) * _tmp28) +
|
||||
_tmp27 * (P(0, 3) * _tmp23 + P(1, 3) * _tmp18 + P(2, 3) * _tmp24 +
|
||||
P(3, 3) * _tmp27 + P(4, 3) * _tmp20 + P(5, 3) * _tmp28) +
|
||||
_tmp28 * (P(0, 5) * _tmp23 + P(1, 5) * _tmp18 + P(2, 5) * _tmp24 +
|
||||
P(3, 5) * _tmp27 + P(4, 5) * _tmp20 + P(5, 5) * _tmp28);
|
||||
_innov_var(1, 0) = R -
|
||||
_tmp29 * (P(0, 3) * _tmp37 + P(1, 3) * _tmp39 + P(2, 3) * _tmp38 -
|
||||
P(3, 3) * _tmp29 - P(4, 3) * _tmp40 - P(5, 3) * _tmp41) +
|
||||
_tmp37 * (P(0, 0) * _tmp37 + P(1, 0) * _tmp39 + P(2, 0) * _tmp38 -
|
||||
P(3, 0) * _tmp29 - P(4, 0) * _tmp40 - P(5, 0) * _tmp41) +
|
||||
_tmp38 * (P(0, 2) * _tmp37 + P(1, 2) * _tmp39 + P(2, 2) * _tmp38 -
|
||||
P(3, 2) * _tmp29 - P(4, 2) * _tmp40 - P(5, 2) * _tmp41) +
|
||||
_tmp39 * (P(0, 1) * _tmp37 + P(1, 1) * _tmp39 + P(2, 1) * _tmp38 -
|
||||
P(3, 1) * _tmp29 - P(4, 1) * _tmp40 - P(5, 1) * _tmp41) -
|
||||
_tmp40 * (P(0, 4) * _tmp37 + P(1, 4) * _tmp39 + P(2, 4) * _tmp38 -
|
||||
P(3, 4) * _tmp29 - P(4, 4) * _tmp40 - P(5, 4) * _tmp41) -
|
||||
_tmp41 * (P(0, 5) * _tmp37 + P(1, 5) * _tmp39 + P(2, 5) * _tmp38 -
|
||||
P(3, 5) * _tmp29 - P(4, 5) * _tmp40 - P(5, 5) * _tmp41);
|
||||
_innov_var(0, 0) =
|
||||
R +
|
||||
_tmp36 * (P(0, 2) * _tmp40 + P(1, 2) * _tmp42 + P(2, 2) * _tmp36 - P(23, 2) * _tmp45 +
|
||||
P(3, 2) * _tmp37 + P(4, 2) * _tmp43 + P(5, 2) * _tmp46 + P(8, 2) * _tmp45) +
|
||||
_tmp37 * (P(0, 3) * _tmp40 + P(1, 3) * _tmp42 + P(2, 3) * _tmp36 - P(23, 3) * _tmp45 +
|
||||
P(3, 3) * _tmp37 + P(4, 3) * _tmp43 + P(5, 3) * _tmp46 + P(8, 3) * _tmp45) +
|
||||
_tmp40 * (P(0, 0) * _tmp40 + P(1, 0) * _tmp42 + P(2, 0) * _tmp36 - P(23, 0) * _tmp45 +
|
||||
P(3, 0) * _tmp37 + P(4, 0) * _tmp43 + P(5, 0) * _tmp46 + P(8, 0) * _tmp45) +
|
||||
_tmp42 * (P(0, 1) * _tmp40 + P(1, 1) * _tmp42 + P(2, 1) * _tmp36 - P(23, 1) * _tmp45 +
|
||||
P(3, 1) * _tmp37 + P(4, 1) * _tmp43 + P(5, 1) * _tmp46 + P(8, 1) * _tmp45) +
|
||||
_tmp43 * (P(0, 4) * _tmp40 + P(1, 4) * _tmp42 + P(2, 4) * _tmp36 - P(23, 4) * _tmp45 +
|
||||
P(3, 4) * _tmp37 + P(4, 4) * _tmp43 + P(5, 4) * _tmp46 + P(8, 4) * _tmp45) -
|
||||
_tmp45 * (P(0, 23) * _tmp40 + P(1, 23) * _tmp42 + P(2, 23) * _tmp36 - P(23, 23) * _tmp45 +
|
||||
P(3, 23) * _tmp37 + P(4, 23) * _tmp43 + P(5, 23) * _tmp46 + P(8, 23) * _tmp45) +
|
||||
_tmp45 * (P(0, 8) * _tmp40 + P(1, 8) * _tmp42 + P(2, 8) * _tmp36 - P(23, 8) * _tmp45 +
|
||||
P(3, 8) * _tmp37 + P(4, 8) * _tmp43 + P(5, 8) * _tmp46 + P(8, 8) * _tmp45) +
|
||||
_tmp46 * (P(0, 5) * _tmp40 + P(1, 5) * _tmp42 + P(2, 5) * _tmp36 - P(23, 5) * _tmp45 +
|
||||
P(3, 5) * _tmp37 + P(4, 5) * _tmp43 + P(5, 5) * _tmp46 + P(8, 5) * _tmp45);
|
||||
_innov_var(1, 0) =
|
||||
R -
|
||||
_tmp48 * (P(0, 5) * _tmp63 + P(1, 5) * _tmp61 + P(2, 5) * _tmp64 + P(23, 5) * _tmp65 -
|
||||
P(3, 5) * _tmp52 - P(4, 5) * _tmp50 - P(5, 5) * _tmp48 - P(8, 5) * _tmp65) -
|
||||
_tmp50 * (P(0, 4) * _tmp63 + P(1, 4) * _tmp61 + P(2, 4) * _tmp64 + P(23, 4) * _tmp65 -
|
||||
P(3, 4) * _tmp52 - P(4, 4) * _tmp50 - P(5, 4) * _tmp48 - P(8, 4) * _tmp65) -
|
||||
_tmp52 * (P(0, 3) * _tmp63 + P(1, 3) * _tmp61 + P(2, 3) * _tmp64 + P(23, 3) * _tmp65 -
|
||||
P(3, 3) * _tmp52 - P(4, 3) * _tmp50 - P(5, 3) * _tmp48 - P(8, 3) * _tmp65) +
|
||||
_tmp61 * (P(0, 1) * _tmp63 + P(1, 1) * _tmp61 + P(2, 1) * _tmp64 + P(23, 1) * _tmp65 -
|
||||
P(3, 1) * _tmp52 - P(4, 1) * _tmp50 - P(5, 1) * _tmp48 - P(8, 1) * _tmp65) +
|
||||
_tmp63 * (P(0, 0) * _tmp63 + P(1, 0) * _tmp61 + P(2, 0) * _tmp64 + P(23, 0) * _tmp65 -
|
||||
P(3, 0) * _tmp52 - P(4, 0) * _tmp50 - P(5, 0) * _tmp48 - P(8, 0) * _tmp65) +
|
||||
_tmp64 * (P(0, 2) * _tmp63 + P(1, 2) * _tmp61 + P(2, 2) * _tmp64 + P(23, 2) * _tmp65 -
|
||||
P(3, 2) * _tmp52 - P(4, 2) * _tmp50 - P(5, 2) * _tmp48 - P(8, 2) * _tmp65) +
|
||||
_tmp65 * (P(0, 23) * _tmp63 + P(1, 23) * _tmp61 + P(2, 23) * _tmp64 + P(23, 23) * _tmp65 -
|
||||
P(3, 23) * _tmp52 - P(4, 23) * _tmp50 - P(5, 23) * _tmp48 - P(8, 23) * _tmp65) -
|
||||
_tmp65 * (P(0, 8) * _tmp63 + P(1, 8) * _tmp61 + P(2, 8) * _tmp64 + P(23, 8) * _tmp65 -
|
||||
P(3, 8) * _tmp52 - P(4, 8) * _tmp50 - P(5, 8) * _tmp48 - P(8, 8) * _tmp65);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp23;
|
||||
_h(1, 0) = _tmp18;
|
||||
_h(2, 0) = _tmp24;
|
||||
_h(3, 0) = _tmp27;
|
||||
_h(4, 0) = _tmp20;
|
||||
_h(5, 0) = _tmp28;
|
||||
_h(0, 0) = _tmp40;
|
||||
_h(1, 0) = _tmp42;
|
||||
_h(2, 0) = _tmp36;
|
||||
_h(3, 0) = _tmp37;
|
||||
_h(4, 0) = _tmp43;
|
||||
_h(5, 0) = _tmp46;
|
||||
_h(8, 0) = _tmp45;
|
||||
_h(23, 0) = -_tmp45;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+72
-57
@@ -16,85 +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: 151
|
||||
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: 232
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (21)
|
||||
const Scalar _tmp0 =
|
||||
Scalar(1.0) /
|
||||
(distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
|
||||
const Scalar _tmp1 =
|
||||
_tmp0 * (-2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1);
|
||||
const Scalar _tmp2 = 4 * state(4, 0);
|
||||
const Scalar _tmp3 = 2 * state(0, 0);
|
||||
const Scalar _tmp4 = 2 * state(6, 0);
|
||||
const Scalar _tmp5 = -_tmp2 * state(3, 0) + _tmp3 * state(5, 0) + _tmp4 * state(1, 0);
|
||||
const Scalar _tmp6 = (Scalar(1) / Scalar(2)) * _tmp0;
|
||||
const Scalar _tmp7 = _tmp5 * _tmp6;
|
||||
const Scalar _tmp8 = 2 * state(1, 0);
|
||||
const Scalar _tmp9 = -_tmp2 * state(2, 0) - _tmp4 * state(0, 0) + _tmp8 * state(5, 0);
|
||||
const Scalar _tmp10 = _tmp6 * _tmp9;
|
||||
const Scalar _tmp11 = 2 * state(5, 0);
|
||||
const Scalar _tmp12 = _tmp6 * (_tmp11 * state(2, 0) + _tmp4 * state(3, 0));
|
||||
const Scalar _tmp13 = _tmp11 * state(3, 0) - _tmp4 * state(2, 0);
|
||||
const Scalar _tmp14 = _tmp6 * state(1, 0);
|
||||
const Scalar _tmp15 =
|
||||
_tmp10 * state(3, 0) - _tmp12 * state(0, 0) + _tmp13 * _tmp14 - _tmp7 * state(2, 0);
|
||||
const Scalar _tmp16 = _tmp13 * _tmp6;
|
||||
const Scalar _tmp17 =
|
||||
_tmp12 * state(2, 0) - _tmp14 * _tmp9 + _tmp16 * state(3, 0) - _tmp7 * state(0, 0);
|
||||
// Intermediate terms (28)
|
||||
const Scalar _tmp0 = 2 * state(0, 0);
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 = -_tmp0 * state(2, 0) + _tmp1 * state(3, 0);
|
||||
const Scalar _tmp3 = 1 - 2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp4 = _tmp3 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp5 = state(24, 0) - state(9, 0);
|
||||
const Scalar _tmp6 =
|
||||
_tmp5 + epsilon * (2 * math::min<Scalar>(0, (((_tmp5) > 0) - ((_tmp5) < 0))) + 1);
|
||||
const Scalar _tmp7 = Scalar(1.0) / (_tmp6);
|
||||
const Scalar _tmp8 = _tmp4 * _tmp7;
|
||||
const Scalar _tmp9 = _tmp2 * _tmp8;
|
||||
const Scalar _tmp10 = _tmp0 * state(3, 0) + _tmp1 * state(2, 0);
|
||||
const Scalar _tmp11 = _tmp10 * _tmp8;
|
||||
const Scalar _tmp12 = _tmp3 - 2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp13 = _tmp12 * _tmp8;
|
||||
const Scalar _tmp14 = _tmp10 * state(5, 0) + _tmp12 * state(4, 0) + _tmp2 * state(6, 0);
|
||||
const Scalar _tmp15 = 4 * _tmp14 * _tmp7;
|
||||
const Scalar _tmp16 = 2 * state(5, 0);
|
||||
const Scalar _tmp17 = 2 * state(6, 0);
|
||||
const Scalar _tmp18 =
|
||||
-_tmp10 * state(0, 0) - _tmp12 * state(3, 0) + _tmp14 * _tmp5 + _tmp16 * state(2, 0);
|
||||
const Scalar _tmp19 = _tmp0 * (_tmp3 * state(3, 0) + _tmp8 * state(2, 0));
|
||||
const Scalar _tmp20 = _tmp0 * (-_tmp3 * state(2, 0) + _tmp8 * state(3, 0));
|
||||
(Scalar(1) / Scalar(2)) * _tmp15 * state(1, 0) -
|
||||
Scalar(1) / Scalar(2) * _tmp8 * (_tmp16 * state(2, 0) + _tmp17 * state(3, 0));
|
||||
const Scalar _tmp19 = 4 * state(4, 0);
|
||||
const Scalar _tmp20 = (Scalar(1) / Scalar(2)) * _tmp15 * state(2, 0) -
|
||||
Scalar(1) / Scalar(2) * _tmp8 *
|
||||
(_tmp16 * state(1, 0) - _tmp17 * state(0, 0) - _tmp19 * state(2, 0));
|
||||
const Scalar _tmp21 = (Scalar(1) / Scalar(2)) * _tmp8;
|
||||
const Scalar _tmp22 =
|
||||
_tmp21 * (_tmp16 * state(0, 0) + _tmp17 * state(1, 0) - _tmp19 * state(3, 0));
|
||||
const Scalar _tmp23 = _tmp21 * (_tmp16 * state(3, 0) - _tmp17 * state(2, 0));
|
||||
const Scalar _tmp24 =
|
||||
_tmp18 * state(3, 0) + _tmp20 * state(0, 0) + _tmp22 * state(1, 0) + _tmp23 * state(2, 0);
|
||||
const Scalar _tmp25 =
|
||||
_tmp18 * state(0, 0) - _tmp20 * state(3, 0) - _tmp22 * state(2, 0) + _tmp23 * state(1, 0);
|
||||
const Scalar _tmp26 =
|
||||
-_tmp18 * state(2, 0) + _tmp20 * state(1, 0) - _tmp22 * state(0, 0) + _tmp23 * state(3, 0);
|
||||
const Scalar _tmp27 = _tmp14 * _tmp4 / std::pow(_tmp6, Scalar(2));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R -
|
||||
_tmp1 * (P(0, 3) * _tmp15 + P(1, 3) * _tmp18 + P(2, 3) * _tmp17 - P(3, 3) * _tmp1 -
|
||||
P(4, 3) * _tmp19 - P(5, 3) * _tmp20) +
|
||||
_tmp15 * (P(0, 0) * _tmp15 + P(1, 0) * _tmp18 + P(2, 0) * _tmp17 -
|
||||
P(3, 0) * _tmp1 - P(4, 0) * _tmp19 - P(5, 0) * _tmp20) +
|
||||
_tmp17 * (P(0, 2) * _tmp15 + P(1, 2) * _tmp18 + P(2, 2) * _tmp17 -
|
||||
P(3, 2) * _tmp1 - P(4, 2) * _tmp19 - P(5, 2) * _tmp20) +
|
||||
_tmp18 * (P(0, 1) * _tmp15 + P(1, 1) * _tmp18 + P(2, 1) * _tmp17 -
|
||||
P(3, 1) * _tmp1 - P(4, 1) * _tmp19 - P(5, 1) * _tmp20) -
|
||||
_tmp19 * (P(0, 4) * _tmp15 + P(1, 4) * _tmp18 + P(2, 4) * _tmp17 -
|
||||
P(3, 4) * _tmp1 - P(4, 4) * _tmp19 - P(5, 4) * _tmp20) -
|
||||
_tmp20 * (P(0, 5) * _tmp15 + P(1, 5) * _tmp18 + P(2, 5) * _tmp17 -
|
||||
P(3, 5) * _tmp1 - P(4, 5) * _tmp19 - P(5, 5) * _tmp20);
|
||||
_innov_var =
|
||||
R -
|
||||
_tmp11 * (P(0, 4) * _tmp25 + P(1, 4) * _tmp24 + P(2, 4) * _tmp26 + P(23, 4) * _tmp27 -
|
||||
P(3, 4) * _tmp13 - P(4, 4) * _tmp11 - P(5, 4) * _tmp9 - P(8, 4) * _tmp27) -
|
||||
_tmp13 * (P(0, 3) * _tmp25 + P(1, 3) * _tmp24 + P(2, 3) * _tmp26 + P(23, 3) * _tmp27 -
|
||||
P(3, 3) * _tmp13 - P(4, 3) * _tmp11 - P(5, 3) * _tmp9 - P(8, 3) * _tmp27) +
|
||||
_tmp24 * (P(0, 1) * _tmp25 + P(1, 1) * _tmp24 + P(2, 1) * _tmp26 + P(23, 1) * _tmp27 -
|
||||
P(3, 1) * _tmp13 - P(4, 1) * _tmp11 - P(5, 1) * _tmp9 - P(8, 1) * _tmp27) +
|
||||
_tmp25 * (P(0, 0) * _tmp25 + P(1, 0) * _tmp24 + P(2, 0) * _tmp26 + P(23, 0) * _tmp27 -
|
||||
P(3, 0) * _tmp13 - P(4, 0) * _tmp11 - P(5, 0) * _tmp9 - P(8, 0) * _tmp27) +
|
||||
_tmp26 * (P(0, 2) * _tmp25 + P(1, 2) * _tmp24 + P(2, 2) * _tmp26 + P(23, 2) * _tmp27 -
|
||||
P(3, 2) * _tmp13 - P(4, 2) * _tmp11 - P(5, 2) * _tmp9 - P(8, 2) * _tmp27) +
|
||||
_tmp27 * (P(0, 23) * _tmp25 + P(1, 23) * _tmp24 + P(2, 23) * _tmp26 + P(23, 23) * _tmp27 -
|
||||
P(3, 23) * _tmp13 - P(4, 23) * _tmp11 - P(5, 23) * _tmp9 - P(8, 23) * _tmp27) -
|
||||
_tmp27 * (P(0, 8) * _tmp25 + P(1, 8) * _tmp24 + P(2, 8) * _tmp26 + P(23, 8) * _tmp27 -
|
||||
P(3, 8) * _tmp13 - P(4, 8) * _tmp11 - P(5, 8) * _tmp9 - P(8, 8) * _tmp27) -
|
||||
_tmp9 * (P(0, 5) * _tmp25 + P(1, 5) * _tmp24 + P(2, 5) * _tmp26 + P(23, 5) * _tmp27 -
|
||||
P(3, 5) * _tmp13 - P(4, 5) * _tmp11 - P(5, 5) * _tmp9 - P(8, 5) * _tmp27);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp15;
|
||||
_h(1, 0) = _tmp18;
|
||||
_h(2, 0) = _tmp17;
|
||||
_h(3, 0) = -_tmp1;
|
||||
_h(4, 0) = -_tmp19;
|
||||
_h(5, 0) = -_tmp20;
|
||||
_h(0, 0) = _tmp25;
|
||||
_h(1, 0) = _tmp24;
|
||||
_h(2, 0) = _tmp26;
|
||||
_h(3, 0) = -_tmp13;
|
||||
_h(4, 0) = -_tmp11;
|
||||
_h(5, 0) = -_tmp9;
|
||||
_h(8, 0) = -_tmp27;
|
||||
_h(23, 0) = _tmp27;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+7
-7
@@ -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: 114
|
||||
|
||||
// Input arrays
|
||||
@@ -93,7 +93,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();
|
||||
|
||||
|
||||
+7
-7
@@ -16,19 +16,19 @@ namespace sym {
|
||||
* Symbolic function: compute_gravity_xyz_innov_var_and_hx
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* R: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Matrix31
|
||||
* Hx: Matrix23_1
|
||||
* Hx: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeGravityXyzInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
void ComputeGravityXyzInnovVarAndHx(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
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: 53
|
||||
|
||||
// Input arrays
|
||||
@@ -61,7 +61,7 @@ void ComputeGravityXyzInnovVarAndHx(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();
|
||||
|
||||
|
||||
+7
-7
@@ -16,19 +16,19 @@ namespace sym {
|
||||
* Symbolic function: compute_gravity_y_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* R: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* Hy: Matrix23_1
|
||||
* Hy: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeGravityYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
void ComputeGravityYInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Hy = nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>* const Hy = nullptr) {
|
||||
// Total ops: 22
|
||||
|
||||
// Input arrays
|
||||
@@ -47,7 +47,7 @@ void ComputeGravityYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
}
|
||||
|
||||
if (Hy != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _hy = (*Hy);
|
||||
matrix::Matrix<Scalar, 24, 1>& _hy = (*Hy);
|
||||
|
||||
_hy.setZero();
|
||||
|
||||
|
||||
+7
-7
@@ -16,19 +16,19 @@ namespace sym {
|
||||
* Symbolic function: compute_gravity_z_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* R: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* Hz: Matrix23_1
|
||||
* Hz: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeGravityZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
void ComputeGravityZInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Hz = nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>* const Hz = nullptr) {
|
||||
// Total ops: 18
|
||||
|
||||
// Input arrays
|
||||
@@ -48,7 +48,7 @@ void ComputeGravityZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
}
|
||||
|
||||
if (Hz != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _hz = (*Hz);
|
||||
matrix::Matrix<Scalar, 24, 1>& _hz = (*Hz);
|
||||
|
||||
_hz.setZero();
|
||||
|
||||
|
||||
@@ -0,0 +1,43 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// This file was autogenerated by symforce from template:
|
||||
// function/FUNCTION.h.jinja
|
||||
// Do NOT modify by hand.
|
||||
// -----------------------------------------------------------------------------
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
namespace sym {
|
||||
|
||||
/**
|
||||
* This function was autogenerated from a symbolic function. Do not modify by hand.
|
||||
*
|
||||
* Symbolic function: compute_hagl_h
|
||||
*
|
||||
* Args:
|
||||
*
|
||||
* Outputs:
|
||||
* H: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeHaglH(matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 0
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (0)
|
||||
|
||||
// Output terms (1)
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(8, 0) = -1;
|
||||
_h(23, 0) = 1;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
@@ -0,0 +1,43 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// This file was autogenerated by symforce from template:
|
||||
// function/FUNCTION.h.jinja
|
||||
// Do NOT modify by hand.
|
||||
// -----------------------------------------------------------------------------
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
namespace sym {
|
||||
|
||||
/**
|
||||
* This function was autogenerated from a symbolic function. Do not modify by hand.
|
||||
*
|
||||
* Symbolic function: compute_hagl_innov_var
|
||||
*
|
||||
* Args:
|
||||
* P: Matrix24_24
|
||||
* R: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeHaglInnovVar(const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
Scalar* const innov_var = nullptr) {
|
||||
// Total ops: 4
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (0)
|
||||
|
||||
// Output terms (1)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = P(23, 23) - P(23, 8) - P(8, 23) + P(8, 8) + R;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
+7
-7
@@ -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();
|
||||
|
||||
|
||||
+7
-7
@@ -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: 461
|
||||
|
||||
// Unused inputs
|
||||
@@ -185,7 +185,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();
|
||||
|
||||
|
||||
+7
-7
@@ -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: 159
|
||||
|
||||
// Unused inputs
|
||||
@@ -85,7 +85,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();
|
||||
|
||||
|
||||
+7
-7
@@ -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: 161
|
||||
|
||||
// Unused inputs
|
||||
@@ -85,7 +85,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,169 +16,172 @@ 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: 497
|
||||
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: 513
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (45)
|
||||
// Intermediate terms (43)
|
||||
const Scalar _tmp0 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp3 = 2 * state(6, 0);
|
||||
const Scalar _tmp4 = _tmp3 * state(0, 0);
|
||||
const Scalar _tmp5 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp6 = _tmp5 - 2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp7 = 2 * state(0, 0);
|
||||
const Scalar _tmp8 = _tmp7 * state(3, 0);
|
||||
const Scalar _tmp9 = 2 * state(2, 0);
|
||||
const Scalar _tmp10 = _tmp9 * state(1, 0);
|
||||
const Scalar _tmp11 = _tmp10 + _tmp8;
|
||||
const Scalar _tmp12 = _tmp1 * state(3, 0) - _tmp9 * state(0, 0);
|
||||
const Scalar _tmp13 = _tmp0 * _tmp11 + _tmp12 * state(6, 0) + _tmp2 * _tmp6;
|
||||
const Scalar _tmp14 =
|
||||
_tmp13 + epsilon * (2 * math::min<Scalar>(0, (((_tmp13) > 0) - ((_tmp13) < 0))) + 1);
|
||||
const Scalar _tmp15 = _tmp5 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp16 = _tmp10 - _tmp8;
|
||||
const Scalar _tmp17 = _tmp7 * state(1, 0) + _tmp9 * state(3, 0);
|
||||
const Scalar _tmp18 =
|
||||
(_tmp0 * _tmp15 + _tmp16 * _tmp2 + _tmp17 * state(6, 0)) / std::pow(_tmp14, Scalar(2));
|
||||
const Scalar _tmp19 = _tmp3 * state(3, 0);
|
||||
const Scalar _tmp20 = Scalar(1.0) / (_tmp14);
|
||||
const Scalar _tmp21 = -_tmp18 * (_tmp0 * _tmp1 - 4 * _tmp2 * state(2, 0) - _tmp4) +
|
||||
_tmp20 * (_tmp1 * _tmp2 + _tmp19);
|
||||
const Scalar _tmp22 = (Scalar(1) / Scalar(2)) * state(3, 0);
|
||||
const Scalar _tmp23 =
|
||||
-Scalar(1) / Scalar(2) * _tmp18 * (_tmp0 * _tmp9 + _tmp19) +
|
||||
(Scalar(1) / Scalar(2)) * _tmp20 * (-4 * _tmp0 * state(1, 0) + _tmp2 * _tmp9 + _tmp4);
|
||||
const Scalar _tmp24 = 2 * state(3, 0);
|
||||
const Scalar _tmp25 = _tmp3 * state(2, 0);
|
||||
const Scalar _tmp26 = _tmp3 * state(1, 0);
|
||||
const Scalar _tmp27 = -_tmp18 * (_tmp0 * _tmp24 - _tmp25) + _tmp20 * (-_tmp2 * _tmp24 + _tmp26);
|
||||
const Scalar _tmp28 = (Scalar(1) / Scalar(2)) * _tmp27;
|
||||
const Scalar _tmp29 = 4 * state(3, 0);
|
||||
const Scalar _tmp3 = 4 * _tmp2;
|
||||
const Scalar _tmp4 = 2 * state(6, 0);
|
||||
const Scalar _tmp5 = _tmp4 * state(0, 0);
|
||||
const Scalar _tmp6 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp7 = _tmp6 - 2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp8 = 2 * state(0, 0);
|
||||
const Scalar _tmp9 = _tmp8 * state(3, 0);
|
||||
const Scalar _tmp10 = 2 * state(2, 0);
|
||||
const Scalar _tmp11 = _tmp10 * state(1, 0);
|
||||
const Scalar _tmp12 = _tmp11 + _tmp9;
|
||||
const Scalar _tmp13 = _tmp1 * state(3, 0) - _tmp10 * state(0, 0);
|
||||
const Scalar _tmp14 = _tmp0 * _tmp12 + _tmp13 * state(6, 0) + _tmp2 * _tmp7;
|
||||
const Scalar _tmp15 =
|
||||
_tmp14 + epsilon * (2 * math::min<Scalar>(0, (((_tmp14) > 0) - ((_tmp14) < 0))) + 1);
|
||||
const Scalar _tmp16 = _tmp6 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp17 = _tmp11 - _tmp9;
|
||||
const Scalar _tmp18 = _tmp10 * state(3, 0) + _tmp8 * state(1, 0);
|
||||
const Scalar _tmp19 =
|
||||
(_tmp0 * _tmp16 + _tmp17 * _tmp2 + _tmp18 * state(6, 0)) / std::pow(_tmp15, Scalar(2));
|
||||
const Scalar _tmp20 = _tmp4 * state(3, 0);
|
||||
const Scalar _tmp21 = Scalar(1.0) / (_tmp15);
|
||||
const Scalar _tmp22 =
|
||||
-Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp1 - _tmp3 * state(2, 0) - _tmp5) +
|
||||
(Scalar(1) / Scalar(2)) * _tmp21 * (_tmp1 * _tmp2 + _tmp20);
|
||||
const Scalar _tmp23 = 4 * _tmp0;
|
||||
const Scalar _tmp24 =
|
||||
-Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp10 + _tmp20) +
|
||||
(Scalar(1) / Scalar(2)) * _tmp21 * (_tmp10 * _tmp2 - _tmp23 * state(1, 0) + _tmp5);
|
||||
const Scalar _tmp25 = 2 * state(3, 0);
|
||||
const Scalar _tmp26 = _tmp4 * state(2, 0);
|
||||
const Scalar _tmp27 = _tmp4 * state(1, 0);
|
||||
const Scalar _tmp28 = -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp25 - _tmp26) +
|
||||
(Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp25 + _tmp27);
|
||||
const Scalar _tmp29 =
|
||||
-Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp8 + _tmp27 - _tmp3 * state(3, 0)) +
|
||||
(Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp8 - _tmp23 * state(3, 0) + _tmp26);
|
||||
const Scalar _tmp30 =
|
||||
-Scalar(1) / Scalar(2) * _tmp18 * (_tmp0 * _tmp7 - _tmp2 * _tmp29 + _tmp26) +
|
||||
(Scalar(1) / Scalar(2)) * _tmp20 * (-_tmp0 * _tmp29 - _tmp2 * _tmp7 + _tmp25);
|
||||
-_tmp22 * state(3, 0) + _tmp24 * state(0, 0) - _tmp28 * state(1, 0) + _tmp29 * state(2, 0);
|
||||
const Scalar _tmp31 =
|
||||
-_tmp21 * _tmp22 + _tmp23 * state(0, 0) - _tmp28 * state(1, 0) + _tmp30 * state(2, 0);
|
||||
const Scalar _tmp32 = (Scalar(1) / Scalar(2)) * _tmp21;
|
||||
const Scalar _tmp33 =
|
||||
_tmp23 * state(3, 0) - _tmp28 * state(2, 0) - _tmp30 * state(1, 0) + _tmp32 * state(0, 0);
|
||||
const Scalar _tmp34 =
|
||||
-_tmp22 * _tmp27 - _tmp23 * state(2, 0) + _tmp30 * state(0, 0) + _tmp32 * state(1, 0);
|
||||
const Scalar _tmp35 = _tmp18 * _tmp6;
|
||||
const Scalar _tmp36 = _tmp16 * _tmp20;
|
||||
const Scalar _tmp37 = -_tmp35 + _tmp36;
|
||||
const Scalar _tmp38 = _tmp11 * _tmp18;
|
||||
const Scalar _tmp39 = _tmp15 * _tmp20;
|
||||
const Scalar _tmp40 = -_tmp38 + _tmp39;
|
||||
const Scalar _tmp41 = -_tmp12 * _tmp18 + _tmp17 * _tmp20;
|
||||
const Scalar _tmp42 = _tmp35 - _tmp36;
|
||||
const Scalar _tmp43 = _tmp38 - _tmp39;
|
||||
const Scalar _tmp44 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
|
||||
_tmp22 * state(0, 0) + _tmp24 * state(3, 0) - _tmp28 * state(2, 0) - _tmp29 * state(1, 0);
|
||||
const Scalar _tmp32 =
|
||||
_tmp22 * state(1, 0) - _tmp24 * state(2, 0) - _tmp28 * state(3, 0) + _tmp29 * state(0, 0);
|
||||
const Scalar _tmp33 = _tmp19 * _tmp7;
|
||||
const Scalar _tmp34 = _tmp17 * _tmp21;
|
||||
const Scalar _tmp35 = -_tmp33 + _tmp34;
|
||||
const Scalar _tmp36 = _tmp12 * _tmp19;
|
||||
const Scalar _tmp37 = _tmp16 * _tmp21;
|
||||
const Scalar _tmp38 = -_tmp36 + _tmp37;
|
||||
const Scalar _tmp39 = -_tmp13 * _tmp19 + _tmp18 * _tmp21;
|
||||
const Scalar _tmp40 = _tmp33 - _tmp34;
|
||||
const Scalar _tmp41 = _tmp36 - _tmp37;
|
||||
const Scalar _tmp42 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
|
||||
|
||||
// Output terms (2)
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp31;
|
||||
_h(1, 0) = _tmp33;
|
||||
_h(2, 0) = _tmp34;
|
||||
_h(3, 0) = _tmp37;
|
||||
_h(4, 0) = _tmp40;
|
||||
_h(5, 0) = _tmp41;
|
||||
_h(21, 0) = _tmp42;
|
||||
_h(22, 0) = _tmp43;
|
||||
_h(0, 0) = _tmp30;
|
||||
_h(1, 0) = _tmp31;
|
||||
_h(2, 0) = _tmp32;
|
||||
_h(3, 0) = _tmp35;
|
||||
_h(4, 0) = _tmp38;
|
||||
_h(5, 0) = _tmp39;
|
||||
_h(21, 0) = _tmp40;
|
||||
_h(22, 0) = _tmp41;
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _k = (*K);
|
||||
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
|
||||
|
||||
_k(0, 0) =
|
||||
_tmp44 * (P(0, 0) * _tmp31 + P(0, 1) * _tmp33 + P(0, 2) * _tmp34 + P(0, 21) * _tmp42 +
|
||||
P(0, 22) * _tmp43 + P(0, 3) * _tmp37 + P(0, 4) * _tmp40 + P(0, 5) * _tmp41);
|
||||
_tmp42 * (P(0, 0) * _tmp30 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 + P(0, 21) * _tmp40 +
|
||||
P(0, 22) * _tmp41 + P(0, 3) * _tmp35 + P(0, 4) * _tmp38 + P(0, 5) * _tmp39);
|
||||
_k(1, 0) =
|
||||
_tmp44 * (P(1, 0) * _tmp31 + P(1, 1) * _tmp33 + P(1, 2) * _tmp34 + P(1, 21) * _tmp42 +
|
||||
P(1, 22) * _tmp43 + P(1, 3) * _tmp37 + P(1, 4) * _tmp40 + P(1, 5) * _tmp41);
|
||||
_tmp42 * (P(1, 0) * _tmp30 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 + P(1, 21) * _tmp40 +
|
||||
P(1, 22) * _tmp41 + P(1, 3) * _tmp35 + P(1, 4) * _tmp38 + P(1, 5) * _tmp39);
|
||||
_k(2, 0) =
|
||||
_tmp44 * (P(2, 0) * _tmp31 + P(2, 1) * _tmp33 + P(2, 2) * _tmp34 + P(2, 21) * _tmp42 +
|
||||
P(2, 22) * _tmp43 + P(2, 3) * _tmp37 + P(2, 4) * _tmp40 + P(2, 5) * _tmp41);
|
||||
_tmp42 * (P(2, 0) * _tmp30 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 + P(2, 21) * _tmp40 +
|
||||
P(2, 22) * _tmp41 + P(2, 3) * _tmp35 + P(2, 4) * _tmp38 + P(2, 5) * _tmp39);
|
||||
_k(3, 0) =
|
||||
_tmp44 * (P(3, 0) * _tmp31 + P(3, 1) * _tmp33 + P(3, 2) * _tmp34 + P(3, 21) * _tmp42 +
|
||||
P(3, 22) * _tmp43 + P(3, 3) * _tmp37 + P(3, 4) * _tmp40 + P(3, 5) * _tmp41);
|
||||
_tmp42 * (P(3, 0) * _tmp30 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 + P(3, 21) * _tmp40 +
|
||||
P(3, 22) * _tmp41 + P(3, 3) * _tmp35 + P(3, 4) * _tmp38 + P(3, 5) * _tmp39);
|
||||
_k(4, 0) =
|
||||
_tmp44 * (P(4, 0) * _tmp31 + P(4, 1) * _tmp33 + P(4, 2) * _tmp34 + P(4, 21) * _tmp42 +
|
||||
P(4, 22) * _tmp43 + P(4, 3) * _tmp37 + P(4, 4) * _tmp40 + P(4, 5) * _tmp41);
|
||||
_tmp42 * (P(4, 0) * _tmp30 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 + P(4, 21) * _tmp40 +
|
||||
P(4, 22) * _tmp41 + P(4, 3) * _tmp35 + P(4, 4) * _tmp38 + P(4, 5) * _tmp39);
|
||||
_k(5, 0) =
|
||||
_tmp44 * (P(5, 0) * _tmp31 + P(5, 1) * _tmp33 + P(5, 2) * _tmp34 + P(5, 21) * _tmp42 +
|
||||
P(5, 22) * _tmp43 + P(5, 3) * _tmp37 + P(5, 4) * _tmp40 + P(5, 5) * _tmp41);
|
||||
_tmp42 * (P(5, 0) * _tmp30 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 + P(5, 21) * _tmp40 +
|
||||
P(5, 22) * _tmp41 + P(5, 3) * _tmp35 + P(5, 4) * _tmp38 + P(5, 5) * _tmp39);
|
||||
_k(6, 0) =
|
||||
_tmp44 * (P(6, 0) * _tmp31 + P(6, 1) * _tmp33 + P(6, 2) * _tmp34 + P(6, 21) * _tmp42 +
|
||||
P(6, 22) * _tmp43 + P(6, 3) * _tmp37 + P(6, 4) * _tmp40 + P(6, 5) * _tmp41);
|
||||
_tmp42 * (P(6, 0) * _tmp30 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 + P(6, 21) * _tmp40 +
|
||||
P(6, 22) * _tmp41 + P(6, 3) * _tmp35 + P(6, 4) * _tmp38 + P(6, 5) * _tmp39);
|
||||
_k(7, 0) =
|
||||
_tmp44 * (P(7, 0) * _tmp31 + P(7, 1) * _tmp33 + P(7, 2) * _tmp34 + P(7, 21) * _tmp42 +
|
||||
P(7, 22) * _tmp43 + P(7, 3) * _tmp37 + P(7, 4) * _tmp40 + P(7, 5) * _tmp41);
|
||||
_tmp42 * (P(7, 0) * _tmp30 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 + P(7, 21) * _tmp40 +
|
||||
P(7, 22) * _tmp41 + P(7, 3) * _tmp35 + P(7, 4) * _tmp38 + P(7, 5) * _tmp39);
|
||||
_k(8, 0) =
|
||||
_tmp44 * (P(8, 0) * _tmp31 + P(8, 1) * _tmp33 + P(8, 2) * _tmp34 + P(8, 21) * _tmp42 +
|
||||
P(8, 22) * _tmp43 + P(8, 3) * _tmp37 + P(8, 4) * _tmp40 + P(8, 5) * _tmp41);
|
||||
_tmp42 * (P(8, 0) * _tmp30 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 + P(8, 21) * _tmp40 +
|
||||
P(8, 22) * _tmp41 + P(8, 3) * _tmp35 + P(8, 4) * _tmp38 + P(8, 5) * _tmp39);
|
||||
_k(9, 0) =
|
||||
_tmp44 * (P(9, 0) * _tmp31 + P(9, 1) * _tmp33 + P(9, 2) * _tmp34 + P(9, 21) * _tmp42 +
|
||||
P(9, 22) * _tmp43 + P(9, 3) * _tmp37 + P(9, 4) * _tmp40 + P(9, 5) * _tmp41);
|
||||
_tmp42 * (P(9, 0) * _tmp30 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 + P(9, 21) * _tmp40 +
|
||||
P(9, 22) * _tmp41 + P(9, 3) * _tmp35 + P(9, 4) * _tmp38 + P(9, 5) * _tmp39);
|
||||
_k(10, 0) =
|
||||
_tmp44 * (P(10, 0) * _tmp31 + P(10, 1) * _tmp33 + P(10, 2) * _tmp34 + P(10, 21) * _tmp42 +
|
||||
P(10, 22) * _tmp43 + P(10, 3) * _tmp37 + P(10, 4) * _tmp40 + P(10, 5) * _tmp41);
|
||||
_tmp42 * (P(10, 0) * _tmp30 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 + P(10, 21) * _tmp40 +
|
||||
P(10, 22) * _tmp41 + P(10, 3) * _tmp35 + P(10, 4) * _tmp38 + P(10, 5) * _tmp39);
|
||||
_k(11, 0) =
|
||||
_tmp44 * (P(11, 0) * _tmp31 + P(11, 1) * _tmp33 + P(11, 2) * _tmp34 + P(11, 21) * _tmp42 +
|
||||
P(11, 22) * _tmp43 + P(11, 3) * _tmp37 + P(11, 4) * _tmp40 + P(11, 5) * _tmp41);
|
||||
_tmp42 * (P(11, 0) * _tmp30 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 + P(11, 21) * _tmp40 +
|
||||
P(11, 22) * _tmp41 + P(11, 3) * _tmp35 + P(11, 4) * _tmp38 + P(11, 5) * _tmp39);
|
||||
_k(12, 0) =
|
||||
_tmp44 * (P(12, 0) * _tmp31 + P(12, 1) * _tmp33 + P(12, 2) * _tmp34 + P(12, 21) * _tmp42 +
|
||||
P(12, 22) * _tmp43 + P(12, 3) * _tmp37 + P(12, 4) * _tmp40 + P(12, 5) * _tmp41);
|
||||
_tmp42 * (P(12, 0) * _tmp30 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 + P(12, 21) * _tmp40 +
|
||||
P(12, 22) * _tmp41 + P(12, 3) * _tmp35 + P(12, 4) * _tmp38 + P(12, 5) * _tmp39);
|
||||
_k(13, 0) =
|
||||
_tmp44 * (P(13, 0) * _tmp31 + P(13, 1) * _tmp33 + P(13, 2) * _tmp34 + P(13, 21) * _tmp42 +
|
||||
P(13, 22) * _tmp43 + P(13, 3) * _tmp37 + P(13, 4) * _tmp40 + P(13, 5) * _tmp41);
|
||||
_tmp42 * (P(13, 0) * _tmp30 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 + P(13, 21) * _tmp40 +
|
||||
P(13, 22) * _tmp41 + P(13, 3) * _tmp35 + P(13, 4) * _tmp38 + P(13, 5) * _tmp39);
|
||||
_k(14, 0) =
|
||||
_tmp44 * (P(14, 0) * _tmp31 + P(14, 1) * _tmp33 + P(14, 2) * _tmp34 + P(14, 21) * _tmp42 +
|
||||
P(14, 22) * _tmp43 + P(14, 3) * _tmp37 + P(14, 4) * _tmp40 + P(14, 5) * _tmp41);
|
||||
_tmp42 * (P(14, 0) * _tmp30 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 + P(14, 21) * _tmp40 +
|
||||
P(14, 22) * _tmp41 + P(14, 3) * _tmp35 + P(14, 4) * _tmp38 + P(14, 5) * _tmp39);
|
||||
_k(15, 0) =
|
||||
_tmp44 * (P(15, 0) * _tmp31 + P(15, 1) * _tmp33 + P(15, 2) * _tmp34 + P(15, 21) * _tmp42 +
|
||||
P(15, 22) * _tmp43 + P(15, 3) * _tmp37 + P(15, 4) * _tmp40 + P(15, 5) * _tmp41);
|
||||
_tmp42 * (P(15, 0) * _tmp30 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 + P(15, 21) * _tmp40 +
|
||||
P(15, 22) * _tmp41 + P(15, 3) * _tmp35 + P(15, 4) * _tmp38 + P(15, 5) * _tmp39);
|
||||
_k(16, 0) =
|
||||
_tmp44 * (P(16, 0) * _tmp31 + P(16, 1) * _tmp33 + P(16, 2) * _tmp34 + P(16, 21) * _tmp42 +
|
||||
P(16, 22) * _tmp43 + P(16, 3) * _tmp37 + P(16, 4) * _tmp40 + P(16, 5) * _tmp41);
|
||||
_tmp42 * (P(16, 0) * _tmp30 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 + P(16, 21) * _tmp40 +
|
||||
P(16, 22) * _tmp41 + P(16, 3) * _tmp35 + P(16, 4) * _tmp38 + P(16, 5) * _tmp39);
|
||||
_k(17, 0) =
|
||||
_tmp44 * (P(17, 0) * _tmp31 + P(17, 1) * _tmp33 + P(17, 2) * _tmp34 + P(17, 21) * _tmp42 +
|
||||
P(17, 22) * _tmp43 + P(17, 3) * _tmp37 + P(17, 4) * _tmp40 + P(17, 5) * _tmp41);
|
||||
_tmp42 * (P(17, 0) * _tmp30 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 + P(17, 21) * _tmp40 +
|
||||
P(17, 22) * _tmp41 + P(17, 3) * _tmp35 + P(17, 4) * _tmp38 + P(17, 5) * _tmp39);
|
||||
_k(18, 0) =
|
||||
_tmp44 * (P(18, 0) * _tmp31 + P(18, 1) * _tmp33 + P(18, 2) * _tmp34 + P(18, 21) * _tmp42 +
|
||||
P(18, 22) * _tmp43 + P(18, 3) * _tmp37 + P(18, 4) * _tmp40 + P(18, 5) * _tmp41);
|
||||
_tmp42 * (P(18, 0) * _tmp30 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 + P(18, 21) * _tmp40 +
|
||||
P(18, 22) * _tmp41 + P(18, 3) * _tmp35 + P(18, 4) * _tmp38 + P(18, 5) * _tmp39);
|
||||
_k(19, 0) =
|
||||
_tmp44 * (P(19, 0) * _tmp31 + P(19, 1) * _tmp33 + P(19, 2) * _tmp34 + P(19, 21) * _tmp42 +
|
||||
P(19, 22) * _tmp43 + P(19, 3) * _tmp37 + P(19, 4) * _tmp40 + P(19, 5) * _tmp41);
|
||||
_tmp42 * (P(19, 0) * _tmp30 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 + P(19, 21) * _tmp40 +
|
||||
P(19, 22) * _tmp41 + P(19, 3) * _tmp35 + P(19, 4) * _tmp38 + P(19, 5) * _tmp39);
|
||||
_k(20, 0) =
|
||||
_tmp44 * (P(20, 0) * _tmp31 + P(20, 1) * _tmp33 + P(20, 2) * _tmp34 + P(20, 21) * _tmp42 +
|
||||
P(20, 22) * _tmp43 + P(20, 3) * _tmp37 + P(20, 4) * _tmp40 + P(20, 5) * _tmp41);
|
||||
_tmp42 * (P(20, 0) * _tmp30 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 + P(20, 21) * _tmp40 +
|
||||
P(20, 22) * _tmp41 + P(20, 3) * _tmp35 + P(20, 4) * _tmp38 + P(20, 5) * _tmp39);
|
||||
_k(21, 0) =
|
||||
_tmp44 * (P(21, 0) * _tmp31 + P(21, 1) * _tmp33 + P(21, 2) * _tmp34 + P(21, 21) * _tmp42 +
|
||||
P(21, 22) * _tmp43 + P(21, 3) * _tmp37 + P(21, 4) * _tmp40 + P(21, 5) * _tmp41);
|
||||
_tmp42 * (P(21, 0) * _tmp30 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 + P(21, 21) * _tmp40 +
|
||||
P(21, 22) * _tmp41 + P(21, 3) * _tmp35 + P(21, 4) * _tmp38 + P(21, 5) * _tmp39);
|
||||
_k(22, 0) =
|
||||
_tmp44 * (P(22, 0) * _tmp31 + P(22, 1) * _tmp33 + P(22, 2) * _tmp34 + P(22, 21) * _tmp42 +
|
||||
P(22, 22) * _tmp43 + P(22, 3) * _tmp37 + P(22, 4) * _tmp40 + P(22, 5) * _tmp41);
|
||||
_tmp42 * (P(22, 0) * _tmp30 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 + P(22, 21) * _tmp40 +
|
||||
P(22, 22) * _tmp41 + P(22, 3) * _tmp35 + P(22, 4) * _tmp38 + P(22, 5) * _tmp39);
|
||||
_k(23, 0) =
|
||||
_tmp42 * (P(23, 0) * _tmp30 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 + P(23, 21) * _tmp40 +
|
||||
P(23, 22) * _tmp41 + P(23, 3) * _tmp35 + P(23, 4) * _tmp38 + P(23, 5) * _tmp39);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+4
-4
@@ -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: 265
|
||||
|
||||
@@ -16,19 +16,19 @@ namespace sym {
|
||||
* Symbolic function: compute_yaw_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* R: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix23_1
|
||||
* H: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeYawInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
void ComputeYawInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 1
|
||||
|
||||
// Unused inputs
|
||||
@@ -46,7 +46,7 @@ void ComputeYawInnovVarAndH(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: predict_covariance
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* state: Matrix25_1
|
||||
* P: Matrix24_24
|
||||
* accel: Matrix31
|
||||
* accel_var: Matrix31
|
||||
* gyro: Matrix31
|
||||
@@ -25,23 +25,23 @@ 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: 1754
|
||||
// Total ops: 1810
|
||||
|
||||
// Unused inputs
|
||||
(void)gyro;
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (147)
|
||||
// Intermediate terms (144)
|
||||
const Scalar _tmp0 = 2 * state(1, 0);
|
||||
const Scalar _tmp1 = _tmp0 * state(3, 0);
|
||||
const Scalar _tmp2 = -_tmp1 * dt;
|
||||
@@ -177,64 +177,62 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
||||
const Scalar _tmp110 = _tmp109 * dt;
|
||||
const Scalar _tmp111 = -_tmp1;
|
||||
const Scalar _tmp112 = _tmp11 + _tmp77;
|
||||
const Scalar _tmp113 =
|
||||
_tmp67 * (_tmp111 + _tmp4) + _tmp72 * (_tmp104 + _tmp112) + _tmp74 * (_tmp106 + _tmp78);
|
||||
const Scalar _tmp114 = _tmp26 * dt;
|
||||
const Scalar _tmp115 = P(0, 4) * _tmp18 + P(10, 4) * _tmp23 + P(11, 4) * _tmp6 + P(9, 4) * _tmp15;
|
||||
const Scalar _tmp116 =
|
||||
const Scalar _tmp113 = dt * (_tmp67 * (_tmp111 + _tmp4) + _tmp72 * (_tmp104 + _tmp112) +
|
||||
_tmp74 * (_tmp106 + _tmp78));
|
||||
const Scalar _tmp114 = P(0, 4) * _tmp18 + P(10, 4) * _tmp23 + P(11, 4) * _tmp6 + P(9, 4) * _tmp15;
|
||||
const Scalar _tmp115 =
|
||||
P(1, 0) * _tmp18 + P(10, 0) * _tmp29 + P(11, 0) * _tmp34 + P(9, 0) * _tmp36;
|
||||
const Scalar _tmp117 = _tmp113 * dt;
|
||||
const Scalar _tmp118 =
|
||||
const Scalar _tmp116 =
|
||||
P(1, 4) * _tmp18 + P(10, 4) * _tmp29 + P(11, 4) * _tmp34 + P(9, 4) * _tmp36;
|
||||
const Scalar _tmp119 =
|
||||
const Scalar _tmp117 =
|
||||
P(10, 0) * _tmp45 + P(11, 0) * _tmp44 + P(2, 0) * _tmp18 + P(9, 0) * _tmp47;
|
||||
const Scalar _tmp120 =
|
||||
const Scalar _tmp118 =
|
||||
P(10, 4) * _tmp45 + P(11, 4) * _tmp44 + P(2, 4) * _tmp18 + P(9, 4) * _tmp47;
|
||||
const Scalar _tmp121 = _tmp56 * _tmp96;
|
||||
const Scalar _tmp122 = _tmp60 * _tmp97;
|
||||
const Scalar _tmp123 = P(1, 0) * _tmp75 - P(12, 0) * _tmp57 - P(13, 0) * _tmp61 -
|
||||
const Scalar _tmp119 = _tmp56 * _tmp96;
|
||||
const Scalar _tmp120 = _tmp60 * _tmp97;
|
||||
const Scalar _tmp121 = P(1, 0) * _tmp75 - P(12, 0) * _tmp57 - P(13, 0) * _tmp61 -
|
||||
P(14, 0) * _tmp64 + P(2, 0) * _tmp79 + P(3, 0);
|
||||
const Scalar _tmp124 = _tmp63 * _tmp98;
|
||||
const Scalar _tmp125 = P(1, 4) * _tmp75 - P(12, 4) * _tmp57 - P(13, 4) * _tmp61 -
|
||||
const Scalar _tmp122 = _tmp63 * _tmp98;
|
||||
const Scalar _tmp123 = P(1, 4) * _tmp75 - P(12, 4) * _tmp57 - P(13, 4) * _tmp61 -
|
||||
P(14, 4) * _tmp64 + P(2, 4) * _tmp79 + P(3, 4);
|
||||
const Scalar _tmp126 = P(0, 0) * _tmp117 - P(12, 0) * _tmp110 - P(13, 0) * _tmp102 -
|
||||
const Scalar _tmp124 = P(0, 0) * _tmp113 - P(12, 0) * _tmp110 - P(13, 0) * _tmp102 -
|
||||
P(14, 0) * _tmp108 + P(2, 0) * _tmp105 + P(4, 0);
|
||||
const Scalar _tmp127 = P(0, 13) * _tmp117 - P(12, 13) * _tmp110 - P(13, 13) * _tmp102 -
|
||||
const Scalar _tmp125 = P(0, 13) * _tmp113 - P(12, 13) * _tmp110 - P(13, 13) * _tmp102 -
|
||||
P(14, 13) * _tmp108 + P(2, 13) * _tmp105 + P(4, 13);
|
||||
const Scalar _tmp128 = P(0, 14) * _tmp117 - P(12, 14) * _tmp110 - P(13, 14) * _tmp102 -
|
||||
const Scalar _tmp126 = P(0, 14) * _tmp113 - P(12, 14) * _tmp110 - P(13, 14) * _tmp102 -
|
||||
P(14, 14) * _tmp108 + P(2, 14) * _tmp105 + P(4, 14);
|
||||
const Scalar _tmp129 = P(0, 12) * _tmp117 - P(12, 12) * _tmp110 - P(13, 12) * _tmp102 -
|
||||
const Scalar _tmp127 = P(0, 12) * _tmp113 - P(12, 12) * _tmp110 - P(13, 12) * _tmp102 -
|
||||
P(14, 12) * _tmp108 + P(2, 12) * _tmp105 + P(4, 12);
|
||||
const Scalar _tmp130 = P(0, 4) * _tmp117 - P(12, 4) * _tmp110 - P(13, 4) * _tmp102 -
|
||||
const Scalar _tmp128 = P(0, 4) * _tmp113 - P(12, 4) * _tmp110 - P(13, 4) * _tmp102 -
|
||||
P(14, 4) * _tmp108 + P(2, 4) * _tmp105 + P(4, 4);
|
||||
const Scalar _tmp131 = _tmp100 + _tmp55;
|
||||
const Scalar _tmp132 = _tmp131 * dt;
|
||||
const Scalar _tmp133 =
|
||||
const Scalar _tmp129 = _tmp100 + _tmp55;
|
||||
const Scalar _tmp130 = _tmp129 * dt;
|
||||
const Scalar _tmp131 =
|
||||
dt * (_tmp67 * (_tmp112 + _tmp69) + _tmp72 * (_tmp111 + _tmp65) + _tmp74 * (_tmp21 + _tmp76));
|
||||
const Scalar _tmp134 = _tmp73 * dt;
|
||||
const Scalar _tmp135 = _tmp66 * dt;
|
||||
const Scalar _tmp136 = _tmp107 * _tmp72 + _tmp109 * _tmp67 + _tmp74 * (_tmp103 + _tmp17 + _tmp68);
|
||||
const Scalar _tmp137 = P(0, 5) * _tmp18 + P(10, 5) * _tmp23 + P(11, 5) * _tmp6 + P(9, 5) * _tmp15;
|
||||
const Scalar _tmp138 = _tmp136 * dt;
|
||||
const Scalar _tmp139 =
|
||||
const Scalar _tmp132 = _tmp73 * dt;
|
||||
const Scalar _tmp133 = _tmp66 * dt;
|
||||
const Scalar _tmp134 =
|
||||
dt * (_tmp107 * _tmp72 + _tmp109 * _tmp67 + _tmp74 * (_tmp103 + _tmp17 + _tmp68));
|
||||
const Scalar _tmp135 = P(0, 5) * _tmp18 + P(10, 5) * _tmp23 + P(11, 5) * _tmp6 + P(9, 5) * _tmp15;
|
||||
const Scalar _tmp136 =
|
||||
P(1, 5) * _tmp18 + P(10, 5) * _tmp29 + P(11, 5) * _tmp34 + P(9, 5) * _tmp36;
|
||||
const Scalar _tmp140 =
|
||||
const Scalar _tmp137 =
|
||||
P(10, 5) * _tmp45 + P(11, 5) * _tmp44 + P(2, 5) * _tmp18 + P(9, 5) * _tmp47;
|
||||
const Scalar _tmp141 = P(1, 5) * _tmp75 - P(12, 5) * _tmp57 - P(13, 5) * _tmp61 -
|
||||
const Scalar _tmp138 = P(1, 5) * _tmp75 - P(12, 5) * _tmp57 - P(13, 5) * _tmp61 -
|
||||
P(14, 5) * _tmp64 + P(2, 5) * _tmp79 + P(3, 5);
|
||||
const Scalar _tmp142 = P(0, 5) * _tmp117 - P(12, 5) * _tmp110 - P(13, 5) * _tmp102 -
|
||||
const Scalar _tmp139 = P(0, 5) * _tmp113 - P(12, 5) * _tmp110 - P(13, 5) * _tmp102 -
|
||||
P(14, 5) * _tmp108 + P(2, 5) * _tmp105 + P(4, 5);
|
||||
const Scalar _tmp143 = P(0, 14) * _tmp138 + P(1, 14) * _tmp133 - P(12, 14) * _tmp135 -
|
||||
P(13, 14) * _tmp134 - P(14, 14) * _tmp132 + P(5, 14);
|
||||
const Scalar _tmp144 = P(0, 13) * _tmp138 + P(1, 13) * _tmp133 - P(12, 13) * _tmp135 -
|
||||
P(13, 13) * _tmp134 - P(14, 13) * _tmp132 + P(5, 13);
|
||||
const Scalar _tmp145 = P(0, 12) * _tmp138 + P(1, 12) * _tmp133 - P(12, 12) * _tmp135 -
|
||||
P(13, 12) * _tmp134 - P(14, 12) * _tmp132 + P(5, 12);
|
||||
const Scalar _tmp146 = P(0, 5) * _tmp138 + P(1, 5) * _tmp133 - P(12, 5) * _tmp135 -
|
||||
P(13, 5) * _tmp134 - P(14, 5) * _tmp132 + P(5, 5);
|
||||
const Scalar _tmp140 = P(0, 14) * _tmp134 + P(1, 14) * _tmp131 - P(12, 14) * _tmp133 -
|
||||
P(13, 14) * _tmp132 - P(14, 14) * _tmp130 + P(5, 14);
|
||||
const Scalar _tmp141 = P(0, 13) * _tmp134 + P(1, 13) * _tmp131 - P(12, 13) * _tmp133 -
|
||||
P(13, 13) * _tmp132 - P(14, 13) * _tmp130 + P(5, 13);
|
||||
const Scalar _tmp142 = P(0, 12) * _tmp134 + P(1, 12) * _tmp131 - P(12, 12) * _tmp133 -
|
||||
P(13, 12) * _tmp132 - P(14, 12) * _tmp130 + P(5, 12);
|
||||
const Scalar _tmp143 = P(0, 5) * _tmp134 + P(1, 5) * _tmp131 - P(12, 5) * _tmp133 -
|
||||
P(13, 5) * _tmp132 - P(14, 5) * _tmp130 + P(5, 5);
|
||||
|
||||
// Output terms (1)
|
||||
matrix::Matrix<Scalar, 23, 23> _res;
|
||||
matrix::Matrix<Scalar, 24, 24> _res;
|
||||
|
||||
_res.setZero();
|
||||
|
||||
@@ -264,40 +262,40 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
||||
std::pow(_tmp63, Scalar(2)) * _tmp98 - _tmp64 * _tmp94 + _tmp75 * _tmp91 +
|
||||
_tmp79 * _tmp92 + _tmp99;
|
||||
_res(0, 4) = -_tmp102 * _tmp58 + _tmp105 * _tmp46 - _tmp108 * _tmp62 - _tmp110 * _tmp53 +
|
||||
_tmp113 * _tmp114 + _tmp115;
|
||||
_tmp113 * _tmp26 + _tmp114;
|
||||
_res(1, 4) = -_tmp102 * _tmp82 + _tmp105 * _tmp48 - _tmp108 * _tmp83 - _tmp110 * _tmp81 +
|
||||
_tmp116 * _tmp117 + _tmp118;
|
||||
_tmp113 * _tmp115 + _tmp116;
|
||||
_res(2, 4) = -_tmp102 * _tmp87 + _tmp105 * _tmp51 - _tmp108 * _tmp88 - _tmp110 * _tmp85 +
|
||||
_tmp117 * _tmp119 + _tmp120;
|
||||
_res(3, 4) = _tmp101 * _tmp122 - _tmp102 * _tmp93 + _tmp105 * _tmp92 + _tmp107 * _tmp124 -
|
||||
_tmp108 * _tmp94 + _tmp109 * _tmp121 - _tmp110 * _tmp90 + _tmp117 * _tmp123 +
|
||||
_tmp125;
|
||||
_res(4, 4) = std::pow(_tmp101, Scalar(2)) * _tmp97 - _tmp102 * _tmp127 +
|
||||
_tmp105 * (P(0, 2) * _tmp117 - P(12, 2) * _tmp110 - P(13, 2) * _tmp102 -
|
||||
_tmp113 * _tmp117 + _tmp118;
|
||||
_res(3, 4) = _tmp101 * _tmp120 - _tmp102 * _tmp93 + _tmp105 * _tmp92 + _tmp107 * _tmp122 -
|
||||
_tmp108 * _tmp94 + _tmp109 * _tmp119 - _tmp110 * _tmp90 + _tmp113 * _tmp121 +
|
||||
_tmp123;
|
||||
_res(4, 4) = std::pow(_tmp101, Scalar(2)) * _tmp97 - _tmp102 * _tmp125 +
|
||||
_tmp105 * (P(0, 2) * _tmp113 - P(12, 2) * _tmp110 - P(13, 2) * _tmp102 -
|
||||
P(14, 2) * _tmp108 + P(2, 2) * _tmp105 + P(4, 2)) +
|
||||
std::pow(_tmp107, Scalar(2)) * _tmp98 - _tmp108 * _tmp128 +
|
||||
std::pow(_tmp109, Scalar(2)) * _tmp96 - _tmp110 * _tmp129 + _tmp117 * _tmp126 +
|
||||
_tmp130;
|
||||
_res(0, 5) = _tmp114 * _tmp136 - _tmp132 * _tmp62 + _tmp133 * _tmp35 - _tmp134 * _tmp58 -
|
||||
_tmp135 * _tmp53 + _tmp137;
|
||||
_res(1, 5) = _tmp116 * _tmp138 - _tmp132 * _tmp83 + _tmp133 * _tmp43 - _tmp134 * _tmp82 -
|
||||
_tmp135 * _tmp81 + _tmp139;
|
||||
_res(2, 5) = _tmp119 * _tmp138 - _tmp132 * _tmp88 + _tmp133 * _tmp86 - _tmp134 * _tmp87 -
|
||||
_tmp135 * _tmp85 + _tmp140;
|
||||
_res(3, 5) = _tmp121 * _tmp66 + _tmp122 * _tmp73 + _tmp123 * _tmp138 + _tmp124 * _tmp131 -
|
||||
_tmp132 * _tmp94 + _tmp133 * _tmp91 - _tmp134 * _tmp93 - _tmp135 * _tmp90 + _tmp141;
|
||||
_res(4, 5) = _tmp101 * _tmp73 * _tmp97 + _tmp107 * _tmp131 * _tmp98 + _tmp109 * _tmp66 * _tmp96 +
|
||||
_tmp126 * _tmp138 - _tmp127 * _tmp134 - _tmp128 * _tmp132 - _tmp129 * _tmp135 +
|
||||
_tmp133 * (P(0, 1) * _tmp117 - P(12, 1) * _tmp110 - P(13, 1) * _tmp102 -
|
||||
std::pow(_tmp107, Scalar(2)) * _tmp98 - _tmp108 * _tmp126 +
|
||||
std::pow(_tmp109, Scalar(2)) * _tmp96 - _tmp110 * _tmp127 + _tmp113 * _tmp124 +
|
||||
_tmp128;
|
||||
_res(0, 5) = -_tmp130 * _tmp62 + _tmp131 * _tmp35 - _tmp132 * _tmp58 - _tmp133 * _tmp53 +
|
||||
_tmp134 * _tmp26 + _tmp135;
|
||||
_res(1, 5) = _tmp115 * _tmp134 - _tmp130 * _tmp83 + _tmp131 * _tmp43 - _tmp132 * _tmp82 -
|
||||
_tmp133 * _tmp81 + _tmp136;
|
||||
_res(2, 5) = _tmp117 * _tmp134 - _tmp130 * _tmp88 + _tmp131 * _tmp86 - _tmp132 * _tmp87 -
|
||||
_tmp133 * _tmp85 + _tmp137;
|
||||
_res(3, 5) = _tmp119 * _tmp66 + _tmp120 * _tmp73 + _tmp121 * _tmp134 + _tmp122 * _tmp129 -
|
||||
_tmp130 * _tmp94 + _tmp131 * _tmp91 - _tmp132 * _tmp93 - _tmp133 * _tmp90 + _tmp138;
|
||||
_res(4, 5) = _tmp101 * _tmp73 * _tmp97 + _tmp107 * _tmp129 * _tmp98 + _tmp109 * _tmp66 * _tmp96 +
|
||||
_tmp124 * _tmp134 - _tmp125 * _tmp132 - _tmp126 * _tmp130 - _tmp127 * _tmp133 +
|
||||
_tmp131 * (P(0, 1) * _tmp113 - P(12, 1) * _tmp110 - P(13, 1) * _tmp102 -
|
||||
P(14, 1) * _tmp108 + P(2, 1) * _tmp105 + P(4, 1)) +
|
||||
_tmp142;
|
||||
_res(5, 5) = std::pow(_tmp131, Scalar(2)) * _tmp98 - _tmp132 * _tmp143 +
|
||||
_tmp133 * (P(0, 1) * _tmp138 + P(1, 1) * _tmp133 - P(12, 1) * _tmp135 -
|
||||
P(13, 1) * _tmp134 - P(14, 1) * _tmp132 + P(5, 1)) -
|
||||
_tmp134 * _tmp144 - _tmp135 * _tmp145 +
|
||||
_tmp138 * (P(0, 0) * _tmp138 + P(1, 0) * _tmp133 - P(12, 0) * _tmp135 -
|
||||
P(13, 0) * _tmp134 - P(14, 0) * _tmp132 + P(5, 0)) +
|
||||
_tmp146 + std::pow(_tmp66, Scalar(2)) * _tmp96 +
|
||||
_tmp139;
|
||||
_res(5, 5) = std::pow(_tmp129, Scalar(2)) * _tmp98 - _tmp130 * _tmp140 +
|
||||
_tmp131 * (P(0, 1) * _tmp134 + P(1, 1) * _tmp131 - P(12, 1) * _tmp133 -
|
||||
P(13, 1) * _tmp132 - P(14, 1) * _tmp130 + P(5, 1)) -
|
||||
_tmp132 * _tmp141 - _tmp133 * _tmp142 +
|
||||
_tmp134 * (P(0, 0) * _tmp134 + P(1, 0) * _tmp131 - P(12, 0) * _tmp133 -
|
||||
P(13, 0) * _tmp132 - P(14, 0) * _tmp130 + P(5, 0)) +
|
||||
_tmp143 + std::pow(_tmp66, Scalar(2)) * _tmp96 +
|
||||
std::pow(_tmp73, Scalar(2)) * _tmp97;
|
||||
_res(0, 6) =
|
||||
P(0, 6) * _tmp18 + P(10, 6) * _tmp23 + P(11, 6) * _tmp6 + P(9, 6) * _tmp15 + _tmp80 * dt;
|
||||
@@ -307,43 +305,43 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
||||
P(10, 6) * _tmp45 + P(11, 6) * _tmp44 + P(2, 6) * _tmp18 + P(9, 6) * _tmp47 + _tmp89 * dt;
|
||||
_res(3, 6) = P(1, 6) * _tmp75 - P(12, 6) * _tmp57 - P(13, 6) * _tmp61 - P(14, 6) * _tmp64 +
|
||||
P(2, 6) * _tmp79 + P(3, 6) + _tmp99 * dt;
|
||||
_res(4, 6) = P(0, 6) * _tmp117 - P(12, 6) * _tmp110 - P(13, 6) * _tmp102 - P(14, 6) * _tmp108 +
|
||||
_res(4, 6) = P(0, 6) * _tmp113 - P(12, 6) * _tmp110 - P(13, 6) * _tmp102 - P(14, 6) * _tmp108 +
|
||||
P(2, 6) * _tmp105 + P(4, 6) +
|
||||
dt * (P(0, 3) * _tmp117 - P(12, 3) * _tmp110 - P(13, 3) * _tmp102 -
|
||||
dt * (P(0, 3) * _tmp113 - P(12, 3) * _tmp110 - P(13, 3) * _tmp102 -
|
||||
P(14, 3) * _tmp108 + P(2, 3) * _tmp105 + P(4, 3));
|
||||
_res(5, 6) = P(0, 6) * _tmp138 + P(1, 6) * _tmp133 - P(12, 6) * _tmp135 - P(13, 6) * _tmp134 -
|
||||
P(14, 6) * _tmp132 + P(5, 6) +
|
||||
dt * (P(0, 3) * _tmp138 + P(1, 3) * _tmp133 - P(12, 3) * _tmp135 -
|
||||
P(13, 3) * _tmp134 - P(14, 3) * _tmp132 + P(5, 3));
|
||||
_res(5, 6) = P(0, 6) * _tmp134 + P(1, 6) * _tmp131 - P(12, 6) * _tmp133 - P(13, 6) * _tmp132 -
|
||||
P(14, 6) * _tmp130 + P(5, 6) +
|
||||
dt * (P(0, 3) * _tmp134 + P(1, 3) * _tmp131 - P(12, 3) * _tmp133 -
|
||||
P(13, 3) * _tmp132 - P(14, 3) * _tmp130 + P(5, 3));
|
||||
_res(6, 6) = P(3, 6) * dt + P(6, 6) + dt * (P(3, 3) * dt + P(6, 3));
|
||||
_res(0, 7) =
|
||||
P(0, 7) * _tmp18 + P(10, 7) * _tmp23 + P(11, 7) * _tmp6 + P(9, 7) * _tmp15 + _tmp115 * dt;
|
||||
P(0, 7) * _tmp18 + P(10, 7) * _tmp23 + P(11, 7) * _tmp6 + P(9, 7) * _tmp15 + _tmp114 * dt;
|
||||
_res(1, 7) =
|
||||
P(1, 7) * _tmp18 + P(10, 7) * _tmp29 + P(11, 7) * _tmp34 + P(9, 7) * _tmp36 + _tmp118 * dt;
|
||||
P(1, 7) * _tmp18 + P(10, 7) * _tmp29 + P(11, 7) * _tmp34 + P(9, 7) * _tmp36 + _tmp116 * dt;
|
||||
_res(2, 7) =
|
||||
P(10, 7) * _tmp45 + P(11, 7) * _tmp44 + P(2, 7) * _tmp18 + P(9, 7) * _tmp47 + _tmp120 * dt;
|
||||
P(10, 7) * _tmp45 + P(11, 7) * _tmp44 + P(2, 7) * _tmp18 + P(9, 7) * _tmp47 + _tmp118 * dt;
|
||||
_res(3, 7) = P(1, 7) * _tmp75 - P(12, 7) * _tmp57 - P(13, 7) * _tmp61 - P(14, 7) * _tmp64 +
|
||||
P(2, 7) * _tmp79 + P(3, 7) + _tmp125 * dt;
|
||||
_res(4, 7) = P(0, 7) * _tmp117 - P(12, 7) * _tmp110 - P(13, 7) * _tmp102 - P(14, 7) * _tmp108 +
|
||||
P(2, 7) * _tmp105 + P(4, 7) + _tmp130 * dt;
|
||||
_res(5, 7) = P(0, 7) * _tmp138 + P(1, 7) * _tmp133 - P(12, 7) * _tmp135 - P(13, 7) * _tmp134 -
|
||||
P(14, 7) * _tmp132 + P(5, 7) +
|
||||
dt * (P(0, 4) * _tmp138 + P(1, 4) * _tmp133 - P(12, 4) * _tmp135 -
|
||||
P(13, 4) * _tmp134 - P(14, 4) * _tmp132 + P(5, 4));
|
||||
P(2, 7) * _tmp79 + P(3, 7) + _tmp123 * dt;
|
||||
_res(4, 7) = P(0, 7) * _tmp113 - P(12, 7) * _tmp110 - P(13, 7) * _tmp102 - P(14, 7) * _tmp108 +
|
||||
P(2, 7) * _tmp105 + P(4, 7) + _tmp128 * dt;
|
||||
_res(5, 7) = P(0, 7) * _tmp134 + P(1, 7) * _tmp131 - P(12, 7) * _tmp133 - P(13, 7) * _tmp132 -
|
||||
P(14, 7) * _tmp130 + P(5, 7) +
|
||||
dt * (P(0, 4) * _tmp134 + P(1, 4) * _tmp131 - P(12, 4) * _tmp133 -
|
||||
P(13, 4) * _tmp132 - P(14, 4) * _tmp130 + 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(0, 8) =
|
||||
P(0, 8) * _tmp18 + P(10, 8) * _tmp23 + P(11, 8) * _tmp6 + P(9, 8) * _tmp15 + _tmp137 * dt;
|
||||
P(0, 8) * _tmp18 + P(10, 8) * _tmp23 + P(11, 8) * _tmp6 + P(9, 8) * _tmp15 + _tmp135 * dt;
|
||||
_res(1, 8) =
|
||||
P(1, 8) * _tmp18 + P(10, 8) * _tmp29 + P(11, 8) * _tmp34 + P(9, 8) * _tmp36 + _tmp139 * dt;
|
||||
P(1, 8) * _tmp18 + P(10, 8) * _tmp29 + P(11, 8) * _tmp34 + P(9, 8) * _tmp36 + _tmp136 * dt;
|
||||
_res(2, 8) =
|
||||
P(10, 8) * _tmp45 + P(11, 8) * _tmp44 + P(2, 8) * _tmp18 + P(9, 8) * _tmp47 + _tmp140 * dt;
|
||||
P(10, 8) * _tmp45 + P(11, 8) * _tmp44 + P(2, 8) * _tmp18 + P(9, 8) * _tmp47 + _tmp137 * dt;
|
||||
_res(3, 8) = P(1, 8) * _tmp75 - P(12, 8) * _tmp57 - P(13, 8) * _tmp61 - P(14, 8) * _tmp64 +
|
||||
P(2, 8) * _tmp79 + P(3, 8) + _tmp141 * dt;
|
||||
_res(4, 8) = P(0, 8) * _tmp117 - P(12, 8) * _tmp110 - P(13, 8) * _tmp102 - P(14, 8) * _tmp108 +
|
||||
P(2, 8) * _tmp105 + P(4, 8) + _tmp142 * dt;
|
||||
_res(5, 8) = P(0, 8) * _tmp138 + P(1, 8) * _tmp133 - P(12, 8) * _tmp135 - P(13, 8) * _tmp134 -
|
||||
P(14, 8) * _tmp132 + P(5, 8) + _tmp146 * dt;
|
||||
P(2, 8) * _tmp79 + P(3, 8) + _tmp138 * dt;
|
||||
_res(4, 8) = P(0, 8) * _tmp113 - P(12, 8) * _tmp110 - P(13, 8) * _tmp102 - P(14, 8) * _tmp108 +
|
||||
P(2, 8) * _tmp105 + P(4, 8) + _tmp139 * dt;
|
||||
_res(5, 8) = P(0, 8) * _tmp134 + P(1, 8) * _tmp131 - P(12, 8) * _tmp133 - P(13, 8) * _tmp132 -
|
||||
P(14, 8) * _tmp130 + P(5, 8) + _tmp143 * 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));
|
||||
@@ -352,10 +350,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
||||
_res(2, 9) = _tmp52;
|
||||
_res(3, 9) = P(1, 9) * _tmp75 - P(12, 9) * _tmp57 - P(13, 9) * _tmp61 - P(14, 9) * _tmp64 +
|
||||
P(2, 9) * _tmp79 + P(3, 9);
|
||||
_res(4, 9) = P(0, 9) * _tmp117 - P(12, 9) * _tmp110 - P(13, 9) * _tmp102 - P(14, 9) * _tmp108 +
|
||||
_res(4, 9) = P(0, 9) * _tmp113 - P(12, 9) * _tmp110 - P(13, 9) * _tmp102 - P(14, 9) * _tmp108 +
|
||||
P(2, 9) * _tmp105 + P(4, 9);
|
||||
_res(5, 9) = P(0, 9) * _tmp138 + P(1, 9) * _tmp133 - P(12, 9) * _tmp135 - P(13, 9) * _tmp134 -
|
||||
P(14, 9) * _tmp132 + P(5, 9);
|
||||
_res(5, 9) = P(0, 9) * _tmp134 + P(1, 9) * _tmp131 - P(12, 9) * _tmp133 - P(13, 9) * _tmp132 -
|
||||
P(14, 9) * _tmp130 + 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);
|
||||
@@ -365,10 +363,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
||||
_res(2, 10) = _tmp49;
|
||||
_res(3, 10) = P(1, 10) * _tmp75 - P(12, 10) * _tmp57 - P(13, 10) * _tmp61 - P(14, 10) * _tmp64 +
|
||||
P(2, 10) * _tmp79 + P(3, 10);
|
||||
_res(4, 10) = P(0, 10) * _tmp117 - P(12, 10) * _tmp110 - P(13, 10) * _tmp102 -
|
||||
_res(4, 10) = P(0, 10) * _tmp113 - P(12, 10) * _tmp110 - P(13, 10) * _tmp102 -
|
||||
P(14, 10) * _tmp108 + P(2, 10) * _tmp105 + P(4, 10);
|
||||
_res(5, 10) = P(0, 10) * _tmp138 + P(1, 10) * _tmp133 - P(12, 10) * _tmp135 -
|
||||
P(13, 10) * _tmp134 - P(14, 10) * _tmp132 + P(5, 10);
|
||||
_res(5, 10) = P(0, 10) * _tmp134 + P(1, 10) * _tmp131 - P(12, 10) * _tmp133 -
|
||||
P(13, 10) * _tmp132 - P(14, 10) * _tmp130 + 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);
|
||||
@@ -379,10 +377,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
||||
_res(2, 11) = _tmp50;
|
||||
_res(3, 11) = P(1, 11) * _tmp75 - P(12, 11) * _tmp57 - P(13, 11) * _tmp61 - P(14, 11) * _tmp64 +
|
||||
P(2, 11) * _tmp79 + P(3, 11);
|
||||
_res(4, 11) = P(0, 11) * _tmp117 - P(12, 11) * _tmp110 - P(13, 11) * _tmp102 -
|
||||
_res(4, 11) = P(0, 11) * _tmp113 - P(12, 11) * _tmp110 - P(13, 11) * _tmp102 -
|
||||
P(14, 11) * _tmp108 + P(2, 11) * _tmp105 + P(4, 11);
|
||||
_res(5, 11) = P(0, 11) * _tmp138 + P(1, 11) * _tmp133 - P(12, 11) * _tmp135 -
|
||||
P(13, 11) * _tmp134 - P(14, 11) * _tmp132 + P(5, 11);
|
||||
_res(5, 11) = P(0, 11) * _tmp134 + P(1, 11) * _tmp131 - P(12, 11) * _tmp133 -
|
||||
P(13, 11) * _tmp132 - P(14, 11) * _tmp130 + 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);
|
||||
@@ -393,8 +391,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
||||
_res(1, 12) = _tmp81;
|
||||
_res(2, 12) = _tmp85;
|
||||
_res(3, 12) = _tmp90;
|
||||
_res(4, 12) = _tmp129;
|
||||
_res(5, 12) = _tmp145;
|
||||
_res(4, 12) = _tmp127;
|
||||
_res(5, 12) = _tmp142;
|
||||
_res(6, 12) = P(3, 12) * dt + P(6, 12);
|
||||
_res(7, 12) = P(4, 12) * dt + P(7, 12);
|
||||
_res(8, 12) = P(5, 12) * dt + P(8, 12);
|
||||
@@ -406,8 +404,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
||||
_res(1, 13) = _tmp82;
|
||||
_res(2, 13) = _tmp87;
|
||||
_res(3, 13) = _tmp93;
|
||||
_res(4, 13) = _tmp127;
|
||||
_res(5, 13) = _tmp144;
|
||||
_res(4, 13) = _tmp125;
|
||||
_res(5, 13) = _tmp141;
|
||||
_res(6, 13) = P(3, 13) * dt + P(6, 13);
|
||||
_res(7, 13) = P(4, 13) * dt + P(7, 13);
|
||||
_res(8, 13) = P(5, 13) * dt + P(8, 13);
|
||||
@@ -420,8 +418,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
||||
_res(1, 14) = _tmp83;
|
||||
_res(2, 14) = _tmp88;
|
||||
_res(3, 14) = _tmp94;
|
||||
_res(4, 14) = _tmp128;
|
||||
_res(5, 14) = _tmp143;
|
||||
_res(4, 14) = _tmp126;
|
||||
_res(5, 14) = _tmp140;
|
||||
_res(6, 14) = P(3, 14) * dt + P(6, 14);
|
||||
_res(7, 14) = P(4, 14) * dt + P(7, 14);
|
||||
_res(8, 14) = P(5, 14) * dt + P(8, 14);
|
||||
@@ -436,10 +434,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
||||
_res(2, 15) = P(10, 15) * _tmp45 + P(11, 15) * _tmp44 + P(2, 15) * _tmp18 + P(9, 15) * _tmp47;
|
||||
_res(3, 15) = P(1, 15) * _tmp75 - P(12, 15) * _tmp57 - P(13, 15) * _tmp61 - P(14, 15) * _tmp64 +
|
||||
P(2, 15) * _tmp79 + P(3, 15);
|
||||
_res(4, 15) = P(0, 15) * _tmp117 - P(12, 15) * _tmp110 - P(13, 15) * _tmp102 -
|
||||
_res(4, 15) = P(0, 15) * _tmp113 - P(12, 15) * _tmp110 - P(13, 15) * _tmp102 -
|
||||
P(14, 15) * _tmp108 + P(2, 15) * _tmp105 + P(4, 15);
|
||||
_res(5, 15) = P(0, 15) * _tmp138 + P(1, 15) * _tmp133 - P(12, 15) * _tmp135 -
|
||||
P(13, 15) * _tmp134 - P(14, 15) * _tmp132 + P(5, 15);
|
||||
_res(5, 15) = P(0, 15) * _tmp134 + P(1, 15) * _tmp131 - P(12, 15) * _tmp133 -
|
||||
P(13, 15) * _tmp132 - P(14, 15) * _tmp130 + 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);
|
||||
@@ -455,10 +453,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
||||
_res(2, 16) = P(10, 16) * _tmp45 + P(11, 16) * _tmp44 + P(2, 16) * _tmp18 + P(9, 16) * _tmp47;
|
||||
_res(3, 16) = P(1, 16) * _tmp75 - P(12, 16) * _tmp57 - P(13, 16) * _tmp61 - P(14, 16) * _tmp64 +
|
||||
P(2, 16) * _tmp79 + P(3, 16);
|
||||
_res(4, 16) = P(0, 16) * _tmp117 - P(12, 16) * _tmp110 - P(13, 16) * _tmp102 -
|
||||
_res(4, 16) = P(0, 16) * _tmp113 - P(12, 16) * _tmp110 - P(13, 16) * _tmp102 -
|
||||
P(14, 16) * _tmp108 + P(2, 16) * _tmp105 + P(4, 16);
|
||||
_res(5, 16) = P(0, 16) * _tmp138 + P(1, 16) * _tmp133 - P(12, 16) * _tmp135 -
|
||||
P(13, 16) * _tmp134 - P(14, 16) * _tmp132 + P(5, 16);
|
||||
_res(5, 16) = P(0, 16) * _tmp134 + P(1, 16) * _tmp131 - P(12, 16) * _tmp133 -
|
||||
P(13, 16) * _tmp132 - P(14, 16) * _tmp130 + 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);
|
||||
@@ -475,10 +473,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
||||
_res(2, 17) = P(10, 17) * _tmp45 + P(11, 17) * _tmp44 + P(2, 17) * _tmp18 + P(9, 17) * _tmp47;
|
||||
_res(3, 17) = P(1, 17) * _tmp75 - P(12, 17) * _tmp57 - P(13, 17) * _tmp61 - P(14, 17) * _tmp64 +
|
||||
P(2, 17) * _tmp79 + P(3, 17);
|
||||
_res(4, 17) = P(0, 17) * _tmp117 - P(12, 17) * _tmp110 - P(13, 17) * _tmp102 -
|
||||
_res(4, 17) = P(0, 17) * _tmp113 - P(12, 17) * _tmp110 - P(13, 17) * _tmp102 -
|
||||
P(14, 17) * _tmp108 + P(2, 17) * _tmp105 + P(4, 17);
|
||||
_res(5, 17) = P(0, 17) * _tmp138 + P(1, 17) * _tmp133 - P(12, 17) * _tmp135 -
|
||||
P(13, 17) * _tmp134 - P(14, 17) * _tmp132 + P(5, 17);
|
||||
_res(5, 17) = P(0, 17) * _tmp134 + P(1, 17) * _tmp131 - P(12, 17) * _tmp133 -
|
||||
P(13, 17) * _tmp132 - P(14, 17) * _tmp130 + 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);
|
||||
@@ -496,10 +494,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
||||
_res(2, 18) = P(10, 18) * _tmp45 + P(11, 18) * _tmp44 + P(2, 18) * _tmp18 + P(9, 18) * _tmp47;
|
||||
_res(3, 18) = P(1, 18) * _tmp75 - P(12, 18) * _tmp57 - P(13, 18) * _tmp61 - P(14, 18) * _tmp64 +
|
||||
P(2, 18) * _tmp79 + P(3, 18);
|
||||
_res(4, 18) = P(0, 18) * _tmp117 - P(12, 18) * _tmp110 - P(13, 18) * _tmp102 -
|
||||
_res(4, 18) = P(0, 18) * _tmp113 - P(12, 18) * _tmp110 - P(13, 18) * _tmp102 -
|
||||
P(14, 18) * _tmp108 + P(2, 18) * _tmp105 + P(4, 18);
|
||||
_res(5, 18) = P(0, 18) * _tmp138 + P(1, 18) * _tmp133 - P(12, 18) * _tmp135 -
|
||||
P(13, 18) * _tmp134 - P(14, 18) * _tmp132 + P(5, 18);
|
||||
_res(5, 18) = P(0, 18) * _tmp134 + P(1, 18) * _tmp131 - P(12, 18) * _tmp133 -
|
||||
P(13, 18) * _tmp132 - P(14, 18) * _tmp130 + 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);
|
||||
@@ -518,10 +516,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
||||
_res(2, 19) = P(10, 19) * _tmp45 + P(11, 19) * _tmp44 + P(2, 19) * _tmp18 + P(9, 19) * _tmp47;
|
||||
_res(3, 19) = P(1, 19) * _tmp75 - P(12, 19) * _tmp57 - P(13, 19) * _tmp61 - P(14, 19) * _tmp64 +
|
||||
P(2, 19) * _tmp79 + P(3, 19);
|
||||
_res(4, 19) = P(0, 19) * _tmp117 - P(12, 19) * _tmp110 - P(13, 19) * _tmp102 -
|
||||
_res(4, 19) = P(0, 19) * _tmp113 - P(12, 19) * _tmp110 - P(13, 19) * _tmp102 -
|
||||
P(14, 19) * _tmp108 + P(2, 19) * _tmp105 + P(4, 19);
|
||||
_res(5, 19) = P(0, 19) * _tmp138 + P(1, 19) * _tmp133 - P(12, 19) * _tmp135 -
|
||||
P(13, 19) * _tmp134 - P(14, 19) * _tmp132 + P(5, 19);
|
||||
_res(5, 19) = P(0, 19) * _tmp134 + P(1, 19) * _tmp131 - P(12, 19) * _tmp133 -
|
||||
P(13, 19) * _tmp132 - P(14, 19) * _tmp130 + 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);
|
||||
@@ -541,10 +539,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
||||
_res(2, 20) = P(10, 20) * _tmp45 + P(11, 20) * _tmp44 + P(2, 20) * _tmp18 + P(9, 20) * _tmp47;
|
||||
_res(3, 20) = P(1, 20) * _tmp75 - P(12, 20) * _tmp57 - P(13, 20) * _tmp61 - P(14, 20) * _tmp64 +
|
||||
P(2, 20) * _tmp79 + P(3, 20);
|
||||
_res(4, 20) = P(0, 20) * _tmp117 - P(12, 20) * _tmp110 - P(13, 20) * _tmp102 -
|
||||
_res(4, 20) = P(0, 20) * _tmp113 - P(12, 20) * _tmp110 - P(13, 20) * _tmp102 -
|
||||
P(14, 20) * _tmp108 + P(2, 20) * _tmp105 + P(4, 20);
|
||||
_res(5, 20) = P(0, 20) * _tmp138 + P(1, 20) * _tmp133 - P(12, 20) * _tmp135 -
|
||||
P(13, 20) * _tmp134 - P(14, 20) * _tmp132 + P(5, 20);
|
||||
_res(5, 20) = P(0, 20) * _tmp134 + P(1, 20) * _tmp131 - P(12, 20) * _tmp133 -
|
||||
P(13, 20) * _tmp132 - P(14, 20) * _tmp130 + 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);
|
||||
@@ -565,10 +563,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
||||
_res(2, 21) = P(10, 21) * _tmp45 + P(11, 21) * _tmp44 + P(2, 21) * _tmp18 + P(9, 21) * _tmp47;
|
||||
_res(3, 21) = P(1, 21) * _tmp75 - P(12, 21) * _tmp57 - P(13, 21) * _tmp61 - P(14, 21) * _tmp64 +
|
||||
P(2, 21) * _tmp79 + P(3, 21);
|
||||
_res(4, 21) = P(0, 21) * _tmp117 - P(12, 21) * _tmp110 - P(13, 21) * _tmp102 -
|
||||
_res(4, 21) = P(0, 21) * _tmp113 - P(12, 21) * _tmp110 - P(13, 21) * _tmp102 -
|
||||
P(14, 21) * _tmp108 + P(2, 21) * _tmp105 + P(4, 21);
|
||||
_res(5, 21) = P(0, 21) * _tmp138 + P(1, 21) * _tmp133 - P(12, 21) * _tmp135 -
|
||||
P(13, 21) * _tmp134 - P(14, 21) * _tmp132 + P(5, 21);
|
||||
_res(5, 21) = P(0, 21) * _tmp134 + P(1, 21) * _tmp131 - P(12, 21) * _tmp133 -
|
||||
P(13, 21) * _tmp132 - P(14, 21) * _tmp130 + 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);
|
||||
@@ -590,10 +588,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
|
||||
_res(2, 22) = P(10, 22) * _tmp45 + P(11, 22) * _tmp44 + P(2, 22) * _tmp18 + P(9, 22) * _tmp47;
|
||||
_res(3, 22) = P(1, 22) * _tmp75 - P(12, 22) * _tmp57 - P(13, 22) * _tmp61 - P(14, 22) * _tmp64 +
|
||||
P(2, 22) * _tmp79 + P(3, 22);
|
||||
_res(4, 22) = P(0, 22) * _tmp117 - P(12, 22) * _tmp110 - P(13, 22) * _tmp102 -
|
||||
_res(4, 22) = P(0, 22) * _tmp113 - P(12, 22) * _tmp110 - P(13, 22) * _tmp102 -
|
||||
P(14, 22) * _tmp108 + P(2, 22) * _tmp105 + P(4, 22);
|
||||
_res(5, 22) = P(0, 22) * _tmp138 + P(1, 22) * _tmp133 - P(12, 22) * _tmp135 -
|
||||
P(13, 22) * _tmp134 - P(14, 22) * _tmp132 + P(5, 22);
|
||||
_res(5, 22) = P(0, 22) * _tmp134 + P(1, 22) * _tmp131 - P(12, 22) * _tmp133 -
|
||||
P(13, 22) * _tmp132 - P(14, 22) * _tmp130 + 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);
|
||||
@@ -611,6 +609,33 @@ 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(0, 23) = P(0, 23) * _tmp18 + P(10, 23) * _tmp23 + P(11, 23) * _tmp6 + P(9, 23) * _tmp15;
|
||||
_res(1, 23) = P(1, 23) * _tmp18 + P(10, 23) * _tmp29 + P(11, 23) * _tmp34 + P(9, 23) * _tmp36;
|
||||
_res(2, 23) = P(10, 23) * _tmp45 + P(11, 23) * _tmp44 + P(2, 23) * _tmp18 + P(9, 23) * _tmp47;
|
||||
_res(3, 23) = P(1, 23) * _tmp75 - P(12, 23) * _tmp57 - P(13, 23) * _tmp61 - P(14, 23) * _tmp64 +
|
||||
P(2, 23) * _tmp79 + P(3, 23);
|
||||
_res(4, 23) = P(0, 23) * _tmp113 - P(12, 23) * _tmp110 - P(13, 23) * _tmp102 -
|
||||
P(14, 23) * _tmp108 + P(2, 23) * _tmp105 + P(4, 23);
|
||||
_res(5, 23) = P(0, 23) * _tmp134 + P(1, 23) * _tmp131 - P(12, 23) * _tmp133 -
|
||||
P(13, 23) * _tmp132 - P(14, 23) * _tmp130 + 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{};
|
||||
|
||||
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;
|
||||
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{23, 1};
|
||||
static constexpr uint8_t size{24};
|
||||
};
|
||||
}
|
||||
#endif // !EKF_STATE_H
|
||||
|
||||
@@ -38,26 +38,17 @@
|
||||
*/
|
||||
|
||||
#include "ekf.h"
|
||||
#include "terrain_estimator/derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h"
|
||||
#include "terrain_estimator/derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h"
|
||||
#include "ekf_derivation/generated/state.h"
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
void Ekf::initHagl()
|
||||
{
|
||||
// assume a ground clearance
|
||||
_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
|
||||
_state.terrain = _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;
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_aid_src_terrain_optical_flow.time_last_fuse = _time_delayed_us;
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.rng_gnd_clearance));
|
||||
}
|
||||
|
||||
void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
|
||||
@@ -74,333 +65,9 @@ void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
|
||||
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)
|
||||
void Ekf::controlHaglRngFusion()
|
||||
{
|
||||
if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder)
|
||||
|| _control_status.flags.rng_fault) {
|
||||
|
||||
stopHaglRngFusion();
|
||||
return;
|
||||
}
|
||||
|
||||
if (_range_sensor.isDataReady()) {
|
||||
updateHaglRng(_aid_src_terrain_range_finder);
|
||||
}
|
||||
|
||||
if (_range_sensor.isDataHealthy()) {
|
||||
|
||||
const bool continuing_conditions_passing = _rng_consistency_check.isKinematicallyConsistent();
|
||||
//&& !_control_status.flags.rng_hgt // TODO: should not be fused when using range height
|
||||
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
&& _range_sensor.isRegularlySendingData()
|
||||
&& (_rng_consistency_check.getTestRatio() < 1.f);
|
||||
|
||||
_time_last_healthy_rng_data = _time_delayed_us;
|
||||
|
||||
if (_hagl_sensor_status.flags.range_finder) {
|
||||
if (continuing_conditions_passing) {
|
||||
fuseHaglRng(_aid_src_terrain_range_finder);
|
||||
|
||||
// We have been rejecting range data for too long
|
||||
const uint64_t timeout = static_cast<uint64_t>(_params.terrain_timeout * 1e6f);
|
||||
const bool is_fusion_failing = isTimedOut(_aid_src_terrain_range_finder.time_last_fuse, timeout);
|
||||
|
||||
if (is_fusion_failing) {
|
||||
if (_range_sensor.getDistBottom() > 2.f * _params.rng_gnd_clearance) {
|
||||
// Data seems good, attempt a reset
|
||||
resetHaglRng();
|
||||
|
||||
} else if (starting_conditions_passing) {
|
||||
// The sensor can probably not detect the ground properly
|
||||
// declare the sensor faulty and stop the fusion
|
||||
_control_status.flags.rng_fault = true;
|
||||
_range_sensor.setFaulty(true);
|
||||
stopHaglRngFusion();
|
||||
|
||||
} else {
|
||||
// This could be a temporary issue, stop the fusion without declaring the sensor faulty
|
||||
stopHaglRngFusion();
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
stopHaglRngFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
_hagl_sensor_status.flags.range_finder = true;
|
||||
|
||||
// Reset the state to the measurement only if the test ratio is large,
|
||||
// otherwise let it converge through the fusion
|
||||
if (_hagl_sensor_status.flags.flow && (_aid_src_terrain_range_finder.test_ratio < 0.2f)) {
|
||||
fuseHaglRng(_aid_src_terrain_range_finder);
|
||||
|
||||
} else {
|
||||
resetHaglRng();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_hagl_sensor_status.flags.range_finder && isTimedOut(_time_last_healthy_rng_data, _params.reset_timeout_max)) {
|
||||
// No data anymore. Stop until it comes back.
|
||||
stopHaglRngFusion();
|
||||
}
|
||||
}
|
||||
|
||||
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();
|
||||
_terrain_var = getRngVar();
|
||||
_terrain_vpos_reset_counter++;
|
||||
|
||||
_aid_src_terrain_range_finder.time_last_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::stopHaglRngFusion()
|
||||
{
|
||||
if (_hagl_sensor_status.flags.range_finder) {
|
||||
ECL_INFO("stopping HAGL range fusion");
|
||||
|
||||
// reset flags
|
||||
_innov_check_fail_status.flags.reject_hagl = false;
|
||||
|
||||
_hagl_sensor_status.flags.range_finder = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::updateHaglRng(estimator_aid_source1d_s &aid_src) const
|
||||
{
|
||||
// get a height above ground measurement from the range finder assuming a flat earth
|
||||
const float meas_hagl = _range_sensor.getDistBottom();
|
||||
|
||||
// predict the hagl from the vehicle position and terrain height
|
||||
const float pred_hagl = _terrain_vpos - _state.pos(2);
|
||||
|
||||
// calculate the observation variance adding the variance of the vehicles own height uncertainty
|
||||
const float obs_variance = getRngVar();
|
||||
|
||||
// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
|
||||
const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
|
||||
|
||||
updateAidSourceStatus(aid_src,
|
||||
_time_delayed_us, // sample timestamp
|
||||
meas_hagl, // observation
|
||||
obs_variance, // observation variance
|
||||
pred_hagl - meas_hagl, // innovation
|
||||
hagl_innov_var, // innovation variance
|
||||
math::max(_params.range_innov_gate, 1.f)); // innovation gate
|
||||
}
|
||||
|
||||
void Ekf::fuseHaglRng(estimator_aid_source1d_s &aid_src)
|
||||
{
|
||||
if (!aid_src.innovation_rejected) {
|
||||
// calculate the Kalman gain
|
||||
const float gain = _terrain_var / aid_src.innovation_variance;
|
||||
|
||||
// correct the state
|
||||
_terrain_vpos -= gain * aid_src.innovation;
|
||||
|
||||
// correct the variance
|
||||
_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
|
||||
|
||||
// record last successful fusion event
|
||||
_innov_check_fail_status.flags.reject_hagl = false;
|
||||
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
aid_src.fused = true;
|
||||
|
||||
} else {
|
||||
_innov_check_fail_status.flags.reject_hagl = true;
|
||||
aid_src.fused = false;
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
void Ekf::controlHaglFlowFusion()
|
||||
{
|
||||
if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow)) {
|
||||
stopHaglFlowFusion();
|
||||
return;
|
||||
}
|
||||
|
||||
if (_flow_data_ready) {
|
||||
updateOptFlow(_aid_src_terrain_optical_flow);
|
||||
|
||||
const bool continuing_conditions_passing = _control_status.flags.in_air
|
||||
&& !_control_status.flags.opt_flow
|
||||
&& _control_status.flags.gps
|
||||
&& !_hagl_sensor_status.flags.range_finder;
|
||||
|
||||
const bool starting_conditions_passing = continuing_conditions_passing;
|
||||
|
||||
if (_hagl_sensor_status.flags.flow) {
|
||||
if (continuing_conditions_passing) {
|
||||
|
||||
// TODO: wait until the midpoint of the flow sample has fallen behind the fusion time horizon
|
||||
fuseFlowForTerrain(_aid_src_terrain_optical_flow);
|
||||
|
||||
// TODO: do something when failing continuously the innovation check
|
||||
/* const bool is_fusion_failing = isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max); */
|
||||
|
||||
/* if (is_fusion_failing) { */
|
||||
/* resetHaglFlow(); */
|
||||
/* } */
|
||||
|
||||
} else {
|
||||
stopHaglFlowFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
_hagl_sensor_status.flags.flow = true;
|
||||
// TODO: do a reset instead of trying to fuse the data?
|
||||
fuseFlowForTerrain(_aid_src_terrain_optical_flow);
|
||||
}
|
||||
}
|
||||
|
||||
_flow_data_ready = false;
|
||||
|
||||
} else if (_hagl_sensor_status.flags.flow && (_time_delayed_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) {
|
||||
// No data anymore. Stop until it comes back.
|
||||
stopHaglFlowFusion();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::stopHaglFlowFusion()
|
||||
{
|
||||
if (_hagl_sensor_status.flags.flow) {
|
||||
ECL_INFO("stopping HAGL flow fusion");
|
||||
|
||||
_hagl_sensor_status.flags.flow = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::resetHaglFlow()
|
||||
{
|
||||
// TODO: use the flow data
|
||||
_terrain_vpos = fmaxf(0.0f, _state.pos(2));
|
||||
_terrain_var = 100.0f;
|
||||
_terrain_vpos_reset_counter++;
|
||||
|
||||
_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;
|
||||
}
|
||||
|
||||
_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
|
||||
@@ -410,11 +77,11 @@ void Ekf::controlHaglFakeFusion()
|
||||
bool recent_terrain_aiding = false;
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
recent_terrain_aiding |= isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)1e6);
|
||||
recent_terrain_aiding |= isRecent(_aid_src_rng_hgt.time_last_fuse, (uint64_t)1e6);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
recent_terrain_aiding |= isRecent(_aid_src_terrain_optical_flow.time_last_fuse, (uint64_t)1e6);
|
||||
recent_terrain_aiding |= isRecent(_aid_src_optical_flow.time_last_fuse, (uint64_t)1e6);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
if (_control_status.flags.vehicle_at_rest || !recent_terrain_aiding) {
|
||||
@@ -425,24 +92,20 @@ void Ekf::controlHaglFakeFusion()
|
||||
|
||||
bool Ekf::isTerrainEstimateValid() const
|
||||
{
|
||||
bool valid = false;
|
||||
// Assume being valid when the uncertainty is small compared to the height above ground
|
||||
bool valid = getTerrainVariance() < sq(0.1f * getHagl());
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
|
||||
// we have been fusing range finder measurements in the last 5 seconds
|
||||
if (isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)5e6)) {
|
||||
if (_hagl_sensor_status.flags.range_finder || !_control_status.flags.in_air) {
|
||||
valid = true;
|
||||
}
|
||||
if (_hagl_sensor_status.flags.range_finder && isRecent(_aid_src_rng_hgt.time_last_fuse, (uint64_t)5e6)) {
|
||||
valid = true;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
|
||||
// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
|
||||
// this can only be the case if the main filter does not fuse optical flow
|
||||
if (_hagl_sensor_status.flags.flow && isRecent(_aid_src_terrain_optical_flow.time_last_fuse, (uint64_t)5e6)) {
|
||||
if (_hagl_sensor_status.flags.flow && isRecent(_aid_src_optical_flow.time_last_fuse, (uint64_t)5e6)) {
|
||||
valid = true;
|
||||
}
|
||||
|
||||
@@ -450,7 +113,3 @@ bool Ekf::isTerrainEstimateValid() const
|
||||
|
||||
return valid;
|
||||
}
|
||||
|
||||
void Ekf::terrainHandleVerticalPositionReset(const float delta_z) {
|
||||
_terrain_vpos += delta_z;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user