mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-20 20:03:54 +08:00
EKF2: Error-State Kalman Filter (#22262)
* ekf derivation: change to error state formulation * ekf2: update auto-generated code for error-state * ekf2: adjust ekf2 code for error state formulation * ekf2_tests: adjust unit tests for error-state EKF * update change indicator for error-state EKF * ekf2_derivation: allow disabling mag and wind states --------- Co-authored-by: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)>
This commit is contained in:
Mathieu Bresciani
committed by
GitHub
GitHub
parent
d7f388e590
commit
0d6c2c8ce9
@@ -4,4 +4,4 @@ uint64 timestamp_sample # the timestamp of the raw data (microseconds)
|
||||
float32[24] states # Internal filter states
|
||||
uint8 n_states # Number of states effectively used
|
||||
|
||||
float32[24] covariances # Diagonal Elements of Covariance Matrix
|
||||
float32[23] covariances # Diagonal Elements of Covariance Matrix
|
||||
|
||||
@@ -153,6 +153,11 @@ public:
|
||||
{
|
||||
(*this).transpose().print();
|
||||
}
|
||||
|
||||
static size_t size()
|
||||
{
|
||||
return M;
|
||||
}
|
||||
};
|
||||
|
||||
template<typename OStream, typename Type, size_t M>
|
||||
|
||||
@@ -68,7 +68,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
|
||||
// monitor the tilt alignment
|
||||
if (!_control_status.flags.tilt_align) {
|
||||
// whilst we are aligning the tilt, monitor the variances
|
||||
const Vector3f angle_err_var_vec = calcRotVecVariances();
|
||||
const Vector3f angle_err_var_vec = getQuaternionVariance();
|
||||
|
||||
// Once the tilt variances have reduced to equivalent of 3deg uncertainty
|
||||
// and declare the tilt alignment complete
|
||||
|
||||
@@ -43,7 +43,6 @@
|
||||
|
||||
#include "ekf.h"
|
||||
#include <ekf_derivation/generated/predict_covariance.h>
|
||||
#include <ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h>
|
||||
|
||||
#include <math.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
@@ -368,10 +367,8 @@ void Ekf::resetQuatCov(const float yaw_noise)
|
||||
|
||||
void Ekf::resetQuatCov(const Vector3f &rot_var_ned)
|
||||
{
|
||||
matrix::SquareMatrix<float, State::quat_nominal.dof> q_cov;
|
||||
sym::RotVarNedToLowerTriangularQuatCov(_state.vector(), rot_var_ned, &q_cov);
|
||||
q_cov.copyLowerToUpperTriangle();
|
||||
resetStateCovariance<State::quat_nominal>(q_cov);
|
||||
matrix::SquareMatrix<float, State::quat_nominal.dof> q_cov_ned = diag(rot_var_ned);
|
||||
resetStateCovariance<State::quat_nominal>(_R_to_earth.T() * q_cov_ned * _R_to_earth);
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
||||
@@ -84,7 +84,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
|
||||
_state.vel(2));
|
||||
const Vector3f rel_wind_body = _state.quat_nominal.rotateVectorInverse(rel_wind_earth);
|
||||
const float rel_wind_speed = rel_wind_body.norm();
|
||||
const VectorState state_vector_prev = _state.vector();
|
||||
const auto state_vector_prev = _state.vector();
|
||||
|
||||
Vector2f bcoef_inv{0.f, 0.f};
|
||||
|
||||
|
||||
@@ -84,8 +84,6 @@ public:
|
||||
// should be called every time new data is pushed into the filter
|
||||
bool update();
|
||||
|
||||
static uint8_t getNumberOfStates() { return State::size; }
|
||||
|
||||
const StateSample &state() const { return _state; }
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
@@ -233,9 +231,6 @@ public:
|
||||
const auto &aid_src_gravity() const { return _aid_src_gravity; }
|
||||
#endif // CONFIG_EKF2_GRAVITY_FUSION
|
||||
|
||||
// get the state vector at the delayed time horizon
|
||||
const matrix::Vector<float, State::size> &getStateAtFusionHorizonAsVector() const { return _state.vector(); }
|
||||
|
||||
#if defined(CONFIG_EKF2_WIND)
|
||||
// get the wind velocity in m/s
|
||||
const Vector2f &getWindVelocity() const { return _state.wind_vel; };
|
||||
@@ -408,8 +403,6 @@ public:
|
||||
// return a bitmask integer that describes which state estimates can be used for flight control
|
||||
void get_ekf_soln_status(uint16_t *status) const;
|
||||
|
||||
// rotate quaternion covariances into variances for an equivalent rotation vector
|
||||
Vector3f calcRotVecVariances() const;
|
||||
float getYawVar() const;
|
||||
|
||||
uint8_t getHeightSensorRef() const { return _height_sensor_ref; }
|
||||
|
||||
@@ -40,8 +40,6 @@
|
||||
*/
|
||||
|
||||
#include "ekf.h"
|
||||
#include <ekf_derivation/generated/quat_var_to_rot_var.h>
|
||||
#include <ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h>
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <lib/world_magnetic_model/geo_mag_declination.h>
|
||||
@@ -758,7 +756,8 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
|
||||
|
||||
void Ekf::fuse(const VectorState &K, float innovation)
|
||||
{
|
||||
_state.quat_nominal -= K.slice<State::quat_nominal.dof, 1>(State::quat_nominal.idx, 0) * innovation;
|
||||
Quatf delta_quat(matrix::AxisAnglef(K.slice<State::quat_nominal.dof, 1>(State::quat_nominal.idx, 0) * (-1.f * innovation)));
|
||||
_state.quat_nominal *= delta_quat;
|
||||
_state.quat_nominal.normalize();
|
||||
_R_to_earth = Dcmf(_state.quat_nominal);
|
||||
|
||||
@@ -851,21 +850,11 @@ void Ekf::updateVerticalDeadReckoningStatus()
|
||||
}
|
||||
}
|
||||
|
||||
// calculate the variances for the rotation vector equivalent
|
||||
Vector3f Ekf::calcRotVecVariances() const
|
||||
{
|
||||
Vector3f rot_var;
|
||||
sym::QuatVarToRotVar(_state.vector(), P, FLT_EPSILON, &rot_var);
|
||||
return rot_var;
|
||||
}
|
||||
|
||||
float Ekf::getYawVar() const
|
||||
{
|
||||
VectorState H_YAW;
|
||||
float yaw_var = 0.f;
|
||||
computeYawInnovVarAndH(0.f, yaw_var, H_YAW);
|
||||
|
||||
return yaw_var;
|
||||
const matrix::SquareMatrix3f rot_cov = diag(getQuaternionVariance());
|
||||
const auto rot_var_ned = matrix::SquareMatrix<float, State::quat_nominal.dof>(_R_to_earth * rot_cov * _R_to_earth.T()).diag();
|
||||
return rot_var_ned(2);
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
@@ -899,7 +888,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
|
||||
const Quatf quat_before_reset = _state.quat_nominal;
|
||||
|
||||
// save a copy of covariance in NED frame to restore it after the quat reset
|
||||
const matrix::SquareMatrix3f rot_cov = diag(calcRotVecVariances());
|
||||
const matrix::SquareMatrix3f rot_cov = diag(getQuaternionVariance());
|
||||
Vector3f rot_var_ned_before_reset = matrix::SquareMatrix3f(_R_to_earth * rot_cov * _R_to_earth.T()).diag();
|
||||
|
||||
// update the yaw angle variance
|
||||
|
||||
@@ -62,7 +62,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
|
||||
|
||||
// Observation jacobian and Kalman gain vectors
|
||||
VectorState H;
|
||||
const VectorState state_vector = _state.vector();
|
||||
const auto state_vector = _state.vector();
|
||||
sym::ComputeMagInnovInnovVarAndHx(state_vector, P, mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H);
|
||||
|
||||
// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
|
||||
|
||||
@@ -94,7 +94,7 @@ void Ekf::fuseOptFlow()
|
||||
// a positive offset in earth frame leads to a smaller height above the ground.
|
||||
float range = predictFlowRange();
|
||||
|
||||
const VectorState state_vector = _state.vector();
|
||||
const auto state_vector = _state.vector();
|
||||
|
||||
Vector2f innov_var;
|
||||
VectorState H;
|
||||
|
||||
@@ -55,7 +55,7 @@ args = parser.parse_args()
|
||||
|
||||
# The state vector is organized in an ordered dictionary
|
||||
State = Values(
|
||||
quat_nominal = sf.V4(),
|
||||
quat_nominal = sf.Rot3(),
|
||||
vel = sf.V3(),
|
||||
pos = sf.V3(),
|
||||
gyro_bias = sf.V3(),
|
||||
@@ -99,9 +99,11 @@ class VTangent(sf.Matrix):
|
||||
class MTangent(sf.Matrix):
|
||||
SHAPE = (State.tangent_dim(), State.tangent_dim())
|
||||
|
||||
def state_to_rot3(state: Values):
|
||||
q = sf.Quaternion(sf.V3(state["quat_nominal"][1], state["quat_nominal"][2], state["quat_nominal"][3]), state["quat_nominal"][0])
|
||||
return sf.Rot3(q)
|
||||
def vstate_to_state(v: VState):
|
||||
state = State.from_storage(v)
|
||||
q_px4 = state["quat_nominal"].to_storage()
|
||||
state["quat_nominal"] = sf.Rot3(sf.Quaternion(xyz=sf.V3(q_px4[1], q_px4[2], q_px4[3]), w=q_px4[0]))
|
||||
return state
|
||||
|
||||
def predict_covariance(
|
||||
state: VState,
|
||||
@@ -114,33 +116,80 @@ def predict_covariance(
|
||||
d_ang_var: sf.Scalar
|
||||
) -> MTangent:
|
||||
|
||||
state = State.from_storage(state)
|
||||
state = vstate_to_state(state)
|
||||
g = sf.Symbol("g") # does not appear in the jacobians
|
||||
|
||||
d_vel_b = state["accel_bias"] * d_vel_dt
|
||||
d_vel_true = d_vel - d_vel_b
|
||||
state_error = Values(
|
||||
theta = sf.V3.symbolic("delta_theta"),
|
||||
vel = sf.V3.symbolic("delta_v"),
|
||||
pos = sf.V3.symbolic("delta_p"),
|
||||
gyro_bias = sf.V3.symbolic("delta_w_b"),
|
||||
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")
|
||||
)
|
||||
|
||||
d_ang_b = state["gyro_bias"] * d_ang_dt
|
||||
d_ang_true = d_ang - d_ang_b
|
||||
if args.disable_mag:
|
||||
del state_error["mag_I"]
|
||||
del state_error["mag_B"]
|
||||
|
||||
q = sf.Quaternion(sf.V3(state["quat_nominal"][1], state["quat_nominal"][2], state["quat_nominal"][3]), state["quat_nominal"][0])
|
||||
R_to_earth = state_to_rot3(state)
|
||||
v = state["vel"]
|
||||
p = state["pos"]
|
||||
if args.disable_wind:
|
||||
del state_error["wind_vel"]
|
||||
|
||||
q_new = q * sf.Quaternion(sf.V3(0.5 * d_ang_true[0], 0.5 * d_ang_true[1], 0.5 * d_ang_true[2]), 1)
|
||||
v_new = v + R_to_earth * d_vel_true + sf.V3(0, 0, g) * d_vel_dt
|
||||
p_new = p + v * d_vel_dt
|
||||
# True state kinematics
|
||||
state_t = Values()
|
||||
|
||||
# Predicted state vector at time t + dt
|
||||
state_new = state.copy()
|
||||
state_new["quat_nominal"] = sf.V4(q_new.w, q_new.x, q_new.y, q_new.z), # convert to Hamiltonian form
|
||||
state_new["vel"] = v_new,
|
||||
state_new["pos"] = p_new,
|
||||
for key in state.keys():
|
||||
if key == "quat_nominal":
|
||||
# Create true quaternion using small angle approximation of the error rotation
|
||||
state_t["quat_nominal"] = state["quat_nominal"] * sf.Rot3(sf.Quaternion(xyz=(0.5 * state_error["theta"]), w=1))
|
||||
else:
|
||||
state_t[key] = state[key] + state_error[key]
|
||||
|
||||
noise = Values(
|
||||
d_vel = sf.V3.symbolic("a_n"),
|
||||
d_ang = sf.V3.symbolic("w_n"),
|
||||
)
|
||||
|
||||
input_t = Values(
|
||||
d_vel = d_vel - state_t["accel_bias"] * d_vel_dt - noise["d_vel"],
|
||||
d_ang = d_ang - state_t["gyro_bias"] * d_ang_dt - noise["d_ang"]
|
||||
)
|
||||
|
||||
R_t = state_t["quat_nominal"]
|
||||
state_t_pred = state_t.copy()
|
||||
state_t_pred["quat_nominal"] = state_t["quat_nominal"] * sf.Rot3(sf.Quaternion(xyz=(0.5 * input_t["d_ang"]), w=1))
|
||||
state_t_pred["vel"] = state_t["vel"] + R_t * input_t["d_vel"] + sf.V3(0, 0, g) * d_vel_dt
|
||||
state_t_pred["pos"] = state_t["pos"] + state_t["vel"] * d_vel_dt
|
||||
|
||||
# Nominal state kinematics
|
||||
input = Values(
|
||||
d_vel = d_vel - state["accel_bias"] * d_vel_dt,
|
||||
d_ang = d_ang - state["gyro_bias"] * d_ang_dt
|
||||
)
|
||||
|
||||
R = state["quat_nominal"]
|
||||
state_pred = state.copy()
|
||||
state_pred["quat_nominal"] = state["quat_nominal"] * sf.Rot3(sf.Quaternion(xyz=(0.5 * input["d_ang"]), w=1))
|
||||
state_pred["vel"] = state["vel"] + R * input["d_vel"] + sf.V3(0, 0, g) * d_vel_dt
|
||||
state_pred["pos"] = state["pos"] + state["vel"] * d_vel_dt
|
||||
|
||||
# Error state kinematics
|
||||
state_error_pred = Values()
|
||||
for key in state_error.keys():
|
||||
if key == "theta":
|
||||
delta_q = sf.Quaternion.from_storage(state_pred["quat_nominal"].to_storage()).conj() * sf.Quaternion.from_storage(state_t_pred["quat_nominal"].to_storage())
|
||||
state_error_pred["theta"] = 2 * sf.V3(delta_q.x, delta_q.y, delta_q.z) # Use small angle approximation to obtain a simpler jacobian
|
||||
else:
|
||||
state_error_pred[key] = state_t_pred[key] - state_pred[key]
|
||||
|
||||
zero_state_error = {state_error[key]: state_error[key].zero() for key in state_error.keys()}
|
||||
zero_noise = {noise[key]: noise[key].zero() for key in noise.keys()}
|
||||
|
||||
# State propagation jacobian
|
||||
A = VState(state_new.to_storage()).jacobian(state, tangent_space = False)
|
||||
G = VState(state_new.to_storage()).jacobian(sf.V6.block_matrix([[d_vel], [d_ang]]), tangent_space = False)
|
||||
A = VTangent(state_error_pred.to_storage()).jacobian(state_error).subs(zero_state_error).subs(zero_noise)
|
||||
G = VTangent(state_error_pred.to_storage()).jacobian(noise).subs(zero_state_error).subs(zero_noise)
|
||||
|
||||
# Covariance propagation
|
||||
var_u = sf.Matrix.diag([d_vel_var[0], d_vel_var[1], d_vel_var[2], d_ang_var, d_ang_var, d_ang_var])
|
||||
@@ -149,8 +198,8 @@ def predict_covariance(
|
||||
# Generate the equations for the upper triangular matrix and the diagonal only
|
||||
# Since the matrix is symmetric, the lower triangle does not need to be derived
|
||||
# and can simply be copied in the implementation
|
||||
for index in range(state.storage_dim()):
|
||||
for j in range(state.storage_dim()):
|
||||
for index in range(state.tangent_dim()):
|
||||
for j in range(state.tangent_dim()):
|
||||
if index > j:
|
||||
P_new[index,j] = 0
|
||||
|
||||
@@ -164,14 +213,14 @@ def compute_airspeed_innov_and_innov_var(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, sf.Scalar):
|
||||
|
||||
state = State.from_storage(state)
|
||||
state = vstate_to_state(state)
|
||||
wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0)
|
||||
vel_rel = state["vel"] - wind
|
||||
airspeed_pred = vel_rel.norm(epsilon=epsilon)
|
||||
|
||||
innov = airspeed_pred - airspeed
|
||||
|
||||
H = sf.V1(airspeed_pred).jacobian(state, tangent_space=False)
|
||||
H = sf.V1(airspeed_pred).jacobian(state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov, innov_var)
|
||||
@@ -183,11 +232,11 @@ def compute_airspeed_h_and_k(
|
||||
epsilon: sf.Scalar
|
||||
) -> (VTangent, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
state = vstate_to_state(state)
|
||||
wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0)
|
||||
vel_rel = state["vel"] - wind
|
||||
airspeed_pred = vel_rel.norm(epsilon=epsilon)
|
||||
H = sf.V1(airspeed_pred).jacobian(state, tangent_space=False)
|
||||
H = sf.V1(airspeed_pred).jacobian(state)
|
||||
|
||||
K = P * H.T / sf.Max(innov_var, epsilon)
|
||||
|
||||
@@ -229,7 +278,7 @@ def predict_sideslip(
|
||||
|
||||
wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0)
|
||||
vel_rel = state["vel"] - wind
|
||||
relative_wind_body = state_to_rot3(state).inverse() * vel_rel
|
||||
relative_wind_body = state["quat_nominal"].inverse() * vel_rel
|
||||
|
||||
# Small angle approximation of side slip model
|
||||
# Protect division by zero using epsilon
|
||||
@@ -244,12 +293,12 @@ def compute_sideslip_innov_and_innov_var(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, sf.Scalar, sf.Scalar):
|
||||
|
||||
state = State.from_storage(state)
|
||||
state = vstate_to_state(state)
|
||||
sideslip_pred = predict_sideslip(state, epsilon);
|
||||
|
||||
innov = sideslip_pred - 0.0
|
||||
|
||||
H = sf.V1(sideslip_pred).jacobian(state, tangent_space=False)
|
||||
H = sf.V1(sideslip_pred).jacobian(state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov, innov_var)
|
||||
@@ -261,10 +310,10 @@ def compute_sideslip_h_and_k(
|
||||
epsilon: sf.Scalar
|
||||
) -> (VTangent, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
state = vstate_to_state(state)
|
||||
sideslip_pred = predict_sideslip(state, epsilon);
|
||||
|
||||
H = sf.V1(sideslip_pred).jacobian(state, tangent_space=False)
|
||||
H = sf.V1(sideslip_pred).jacobian(state)
|
||||
|
||||
K = P * H.T / sf.Max(innov_var, epsilon)
|
||||
|
||||
@@ -274,7 +323,7 @@ def predict_mag_body(state) -> sf.V3:
|
||||
mag_field_earth = state["mag_I"]
|
||||
mag_bias_body = state["mag_B"]
|
||||
|
||||
mag_body = state_to_rot3(state).inverse() * mag_field_earth + mag_bias_body
|
||||
mag_body = state["quat_nominal"].inverse() * mag_field_earth + mag_bias_body
|
||||
return mag_body
|
||||
|
||||
def compute_mag_innov_innov_var_and_hx(
|
||||
@@ -285,17 +334,17 @@ def compute_mag_innov_innov_var_and_hx(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.V3, sf.V3, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_mag_body(state);
|
||||
|
||||
innov = meas_pred - meas
|
||||
|
||||
innov_var = sf.V3()
|
||||
Hx = sf.V1(meas_pred[0]).jacobian(state, tangent_space=False)
|
||||
Hx = sf.V1(meas_pred[0]).jacobian(state)
|
||||
innov_var[0] = (Hx * P * Hx.T + R)[0,0]
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False)
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state)
|
||||
innov_var[1] = (Hy * P * Hy.T + R)[0,0]
|
||||
Hz = sf.V1(meas_pred[2]).jacobian(state, tangent_space=False)
|
||||
Hz = sf.V1(meas_pred[2]).jacobian(state)
|
||||
innov_var[2] = (Hz * P * Hz.T + R)[0,0]
|
||||
|
||||
return (innov, innov_var, Hx.T)
|
||||
@@ -307,10 +356,10 @@ def compute_mag_y_innov_var_and_h(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_mag_body(state);
|
||||
|
||||
H = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False)
|
||||
H = sf.V1(meas_pred[1]).jacobian(state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
@@ -322,10 +371,10 @@ def compute_mag_z_innov_var_and_h(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_mag_body(state);
|
||||
|
||||
H = sf.V1(meas_pred[2]).jacobian(state, tangent_space=False)
|
||||
H = sf.V1(meas_pred[2]).jacobian(state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
@@ -337,12 +386,12 @@ def compute_yaw_321_innov_var_and_h(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
R_to_earth = state_to_rot3(state).to_rotation_matrix()
|
||||
state = vstate_to_state(state)
|
||||
R_to_earth = state["quat_nominal"].to_rotation_matrix()
|
||||
# Fix the singularity at pi/2 by inserting epsilon
|
||||
meas_pred = sf.atan2(R_to_earth[1,0], R_to_earth[0,0], epsilon=epsilon)
|
||||
|
||||
H = sf.V1(meas_pred).jacobian(state, tangent_space=False)
|
||||
H = sf.V1(meas_pred).jacobian(state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
@@ -354,12 +403,12 @@ def compute_yaw_321_innov_var_and_h_alternate(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
R_to_earth = state_to_rot3(state).to_rotation_matrix()
|
||||
state = vstate_to_state(state)
|
||||
R_to_earth = state["quat_nominal"].to_rotation_matrix()
|
||||
# Alternate form that has a singularity at yaw 0 instead of pi/2
|
||||
meas_pred = sf.pi/2 - sf.atan2(R_to_earth[0,0], R_to_earth[1,0], epsilon=epsilon)
|
||||
|
||||
H = sf.V1(meas_pred).jacobian(state, tangent_space=False)
|
||||
H = sf.V1(meas_pred).jacobian(state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
@@ -371,12 +420,12 @@ def compute_yaw_312_innov_var_and_h(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
R_to_earth = state_to_rot3(state).to_rotation_matrix()
|
||||
state = vstate_to_state(state)
|
||||
R_to_earth = state["quat_nominal"].to_rotation_matrix()
|
||||
# Alternate form to be used when close to pitch +-pi/2
|
||||
meas_pred = sf.atan2(-R_to_earth[0,1], R_to_earth[1,1], epsilon=epsilon)
|
||||
|
||||
H = sf.V1(meas_pred).jacobian(state, tangent_space=False)
|
||||
H = sf.V1(meas_pred).jacobian(state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
@@ -388,12 +437,12 @@ def compute_yaw_312_innov_var_and_h_alternate(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
R_to_earth = state_to_rot3(state).to_rotation_matrix()
|
||||
state = vstate_to_state(state)
|
||||
R_to_earth = state["quat_nominal"].to_rotation_matrix()
|
||||
# Alternate form to be used when close to pitch +-pi/2
|
||||
meas_pred = sf.pi/2 - sf.atan2(-R_to_earth[1,1], R_to_earth[0,1], epsilon=epsilon)
|
||||
|
||||
H = sf.V1(meas_pred).jacobian(state, tangent_space=False)
|
||||
H = sf.V1(meas_pred).jacobian(state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
@@ -405,16 +454,16 @@ def compute_mag_declination_pred_innov_var_and_h(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = sf.atan2(state["mag_I"][1], state["mag_I"][0], epsilon=epsilon)
|
||||
|
||||
H = sf.V1(meas_pred).jacobian(state, tangent_space=False)
|
||||
H = sf.V1(meas_pred).jacobian(state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (meas_pred, innov_var, H.T)
|
||||
|
||||
def predict_opt_flow(state, distance, epsilon):
|
||||
R_to_body = state_to_rot3(state).inverse()
|
||||
R_to_body = state["quat_nominal"].inverse()
|
||||
|
||||
# Calculate earth relative velocity in a non-rotating sensor frame
|
||||
rel_vel_sensor = R_to_body * state["vel"]
|
||||
@@ -436,13 +485,13 @@ def compute_flow_xy_innov_var_and_hx(
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.V2, VTangent):
|
||||
state = State.from_storage(state)
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_opt_flow(state, distance, epsilon);
|
||||
|
||||
innov_var = sf.V2()
|
||||
Hx = sf.V1(meas_pred[0]).jacobian(state, tangent_space=False)
|
||||
Hx = sf.V1(meas_pred[0]).jacobian(state)
|
||||
innov_var[0] = (Hx * P * Hx.T + R)[0,0]
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False)
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state)
|
||||
innov_var[1] = (Hy * P * Hy.T + R)[0,0]
|
||||
|
||||
return (innov_var, Hx.T)
|
||||
@@ -454,10 +503,10 @@ def compute_flow_y_innov_var_and_h(
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VTangent):
|
||||
state = State.from_storage(state)
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_opt_flow(state, distance, epsilon);
|
||||
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False)
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state)
|
||||
innov_var = (Hy * P * Hy.T + R)[0,0]
|
||||
|
||||
return (innov_var, Hy.T)
|
||||
@@ -470,8 +519,8 @@ def compute_gnss_yaw_pred_innov_var_and_h(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
R_to_earth = state_to_rot3(state)
|
||||
state = vstate_to_state(state)
|
||||
R_to_earth = state["quat_nominal"]
|
||||
|
||||
# define antenna vector in body frame
|
||||
ant_vec_bf = sf.V3(sf.cos(antenna_yaw_offset), sf.sin(antenna_yaw_offset), 0)
|
||||
@@ -482,7 +531,7 @@ def compute_gnss_yaw_pred_innov_var_and_h(
|
||||
# Calculate the yaw angle from the projection
|
||||
meas_pred = sf.atan2(ant_vec_ef[1], ant_vec_ef[0], epsilon=epsilon)
|
||||
|
||||
H = sf.V1(meas_pred).jacobian(state, tangent_space=False)
|
||||
H = sf.V1(meas_pred).jacobian(state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (meas_pred, innov_var, H.T)
|
||||
@@ -494,7 +543,7 @@ def predict_drag(
|
||||
cm: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar):
|
||||
R_to_body = state_to_rot3(state).inverse()
|
||||
R_to_body = state["quat_nominal"].inverse()
|
||||
|
||||
wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0)
|
||||
vel_rel = state["vel"] - wind
|
||||
@@ -517,9 +566,9 @@ def compute_drag_x_innov_var_and_k(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_drag(state, rho, cd, cm, epsilon)
|
||||
Hx = sf.V1(meas_pred[0]).jacobian(state, tangent_space=False)
|
||||
Hx = sf.V1(meas_pred[0]).jacobian(state)
|
||||
innov_var = (Hx * P * Hx.T + R)[0,0]
|
||||
Ktotal = P * Hx.T / sf.Max(innov_var, epsilon)
|
||||
K = VTangent()
|
||||
@@ -537,9 +586,9 @@ def compute_drag_y_innov_var_and_k(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_drag(state, rho, cd, cm, epsilon)
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False)
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state)
|
||||
innov_var = (Hy * P * Hy.T + R)[0,0]
|
||||
Ktotal = P * Hy.T / sf.Max(innov_var, epsilon)
|
||||
K = VTangent()
|
||||
@@ -555,9 +604,9 @@ def compute_gravity_innov_var_and_k_and_h(
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.V3, sf.V3, VTangent, VTangent, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
state = vstate_to_state(state)
|
||||
# get transform from earth to body frame
|
||||
R_to_body = state_to_rot3(state).inverse()
|
||||
R_to_body = state["quat_nominal"].inverse()
|
||||
|
||||
# the innovation is the error between measured acceleration
|
||||
# and predicted (body frame), assuming no body acceleration
|
||||
@@ -571,46 +620,12 @@ def compute_gravity_innov_var_and_k_and_h(
|
||||
# calculate observation jacobian (H), kalman gain (K), and innovation variance (S)
|
||||
# for each axis
|
||||
for i in range(3):
|
||||
H = sf.V1(meas_pred[i]).jacobian(state, tangent_space=False)
|
||||
H = sf.V1(meas_pred[i]).jacobian(state)
|
||||
innov_var[i] = (H * P * H.T + R)[0,0]
|
||||
K[i] = P * H.T / innov_var[i]
|
||||
|
||||
return (innov, innov_var, K[0], K[1], K[2])
|
||||
|
||||
def quat_var_to_rot_var(
|
||||
state: VState,
|
||||
P: MTangent,
|
||||
epsilon: sf.Scalar
|
||||
) -> sf.V3:
|
||||
state = State.from_storage(state)
|
||||
J = sf.V3(state_to_rot3(state).to_tangent(epsilon=epsilon)).jacobian(state, tangent_space=False)
|
||||
rot_cov = J * P * J.T
|
||||
return sf.V3(rot_cov[0, 0], rot_cov[1, 1], rot_cov[2, 2])
|
||||
|
||||
def rot_var_ned_to_lower_triangular_quat_cov(
|
||||
state: VState,
|
||||
rot_var_ned: sf.V3
|
||||
) -> sf.M44:
|
||||
# This function converts an attitude variance defined by a 3D vector in NED frame
|
||||
# into a 4x4 covariance matrix representing the uncertainty on each of the 4 quaternion parameters
|
||||
# Note: the resulting quaternion uncertainty is defined as a perturbation
|
||||
# at the tip of the quaternion (i.e.:body-frame uncertainty)
|
||||
state = State.from_storage(state)
|
||||
q = state["quat_nominal"]
|
||||
attitude = state_to_rot3(state)
|
||||
J = q.jacobian(attitude)
|
||||
|
||||
# Convert uncertainties from NED to body frame
|
||||
rot_cov_ned = sf.M33.diag(rot_var_ned)
|
||||
adjoint = attitude.to_rotation_matrix() # the adjoint of SO(3) is simply the rotation matrix itself
|
||||
rot_cov_body = adjoint.T * rot_cov_ned * adjoint
|
||||
|
||||
# Convert yaw (body) to quaternion parameter uncertainty
|
||||
q_var = J * rot_cov_body * J.T
|
||||
|
||||
# Generate lower trangle only and copy it to the upper part in implementation (produces less code)
|
||||
return q_var.lower_triangle()
|
||||
|
||||
print("Derive EKF2 equations...")
|
||||
generate_px4_function(predict_covariance, output_names=None)
|
||||
|
||||
@@ -638,7 +653,4 @@ generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var",
|
||||
generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "H"])
|
||||
generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"])
|
||||
|
||||
generate_px4_function(quat_var_to_rot_var, output_names=["rot_var"])
|
||||
generate_px4_function(rot_var_ned_to_lower_triangular_quat_cov, output_names=["q_cov_lower_triangle"])
|
||||
|
||||
generate_px4_state(State, tangent_idx)
|
||||
|
||||
@@ -104,7 +104,7 @@ def build_state_struct(state, T="float"):
|
||||
raise NotImplementedError
|
||||
|
||||
for key, val in state.items():
|
||||
out += f"\t{TypeFromLength(len(val))} {key}{{}};\n"
|
||||
out += f"\t{TypeFromLength(val.storage_dim())} {key}{{}};\n"
|
||||
|
||||
state_size = state.storage_dim()
|
||||
out += f"\n\tmatrix::Vector<{T}, {state_size}> Data() const {{\n" \
|
||||
|
||||
@@ -17,20 +17,20 @@ namespace sym {
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* P: Matrix23_23
|
||||
* innov_var: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* H: Matrix24_1
|
||||
* K: Matrix24_1
|
||||
* H: Matrix23_1
|
||||
* K: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
|
||||
const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
|
||||
// Total ops: 256
|
||||
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
|
||||
|
||||
// Input arrays
|
||||
|
||||
@@ -47,68 +47,66 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
|
||||
// Output terms (2)
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(4, 0) = _tmp3;
|
||||
_h(5, 0) = _tmp4;
|
||||
_h(6, 0) = _tmp5;
|
||||
_h(22, 0) = -_tmp3;
|
||||
_h(23, 0) = -_tmp4;
|
||||
_h(3, 0) = _tmp3;
|
||||
_h(4, 0) = _tmp4;
|
||||
_h(5, 0) = _tmp5;
|
||||
_h(21, 0) = -_tmp3;
|
||||
_h(22, 0) = -_tmp4;
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
|
||||
matrix::Matrix<Scalar, 23, 1>& _k = (*K);
|
||||
|
||||
_k(0, 0) = _tmp6 * (-P(0, 22) * _tmp3 - P(0, 23) * _tmp4 + P(0, 4) * _tmp3 + P(0, 5) * _tmp4 +
|
||||
P(0, 6) * _tmp5);
|
||||
_k(1, 0) = _tmp6 * (-P(1, 22) * _tmp3 - P(1, 23) * _tmp4 + P(1, 4) * _tmp3 + P(1, 5) * _tmp4 +
|
||||
P(1, 6) * _tmp5);
|
||||
_k(2, 0) = _tmp6 * (-P(2, 22) * _tmp3 - P(2, 23) * _tmp4 + P(2, 4) * _tmp3 + P(2, 5) * _tmp4 +
|
||||
P(2, 6) * _tmp5);
|
||||
_k(3, 0) = _tmp6 * (-P(3, 22) * _tmp3 - P(3, 23) * _tmp4 + P(3, 4) * _tmp3 + P(3, 5) * _tmp4 +
|
||||
P(3, 6) * _tmp5);
|
||||
_k(4, 0) = _tmp6 * (-P(4, 22) * _tmp3 - P(4, 23) * _tmp4 + P(4, 4) * _tmp3 + P(4, 5) * _tmp4 +
|
||||
P(4, 6) * _tmp5);
|
||||
_k(5, 0) = _tmp6 * (-P(5, 22) * _tmp3 - P(5, 23) * _tmp4 + P(5, 4) * _tmp3 + P(5, 5) * _tmp4 +
|
||||
P(5, 6) * _tmp5);
|
||||
_k(6, 0) = _tmp6 * (-P(6, 22) * _tmp3 - P(6, 23) * _tmp4 + P(6, 4) * _tmp3 + P(6, 5) * _tmp4 +
|
||||
P(6, 6) * _tmp5);
|
||||
_k(7, 0) = _tmp6 * (-P(7, 22) * _tmp3 - P(7, 23) * _tmp4 + P(7, 4) * _tmp3 + P(7, 5) * _tmp4 +
|
||||
P(7, 6) * _tmp5);
|
||||
_k(8, 0) = _tmp6 * (-P(8, 22) * _tmp3 - P(8, 23) * _tmp4 + P(8, 4) * _tmp3 + P(8, 5) * _tmp4 +
|
||||
P(8, 6) * _tmp5);
|
||||
_k(9, 0) = _tmp6 * (-P(9, 22) * _tmp3 - P(9, 23) * _tmp4 + P(9, 4) * _tmp3 + P(9, 5) * _tmp4 +
|
||||
P(9, 6) * _tmp5);
|
||||
_k(10, 0) = _tmp6 * (-P(10, 22) * _tmp3 - P(10, 23) * _tmp4 + P(10, 4) * _tmp3 +
|
||||
P(10, 5) * _tmp4 + P(10, 6) * _tmp5);
|
||||
_k(11, 0) = _tmp6 * (-P(11, 22) * _tmp3 - P(11, 23) * _tmp4 + P(11, 4) * _tmp3 +
|
||||
P(11, 5) * _tmp4 + P(11, 6) * _tmp5);
|
||||
_k(12, 0) = _tmp6 * (-P(12, 22) * _tmp3 - P(12, 23) * _tmp4 + P(12, 4) * _tmp3 +
|
||||
P(12, 5) * _tmp4 + P(12, 6) * _tmp5);
|
||||
_k(13, 0) = _tmp6 * (-P(13, 22) * _tmp3 - P(13, 23) * _tmp4 + P(13, 4) * _tmp3 +
|
||||
P(13, 5) * _tmp4 + P(13, 6) * _tmp5);
|
||||
_k(14, 0) = _tmp6 * (-P(14, 22) * _tmp3 - P(14, 23) * _tmp4 + P(14, 4) * _tmp3 +
|
||||
P(14, 5) * _tmp4 + P(14, 6) * _tmp5);
|
||||
_k(15, 0) = _tmp6 * (-P(15, 22) * _tmp3 - P(15, 23) * _tmp4 + P(15, 4) * _tmp3 +
|
||||
P(15, 5) * _tmp4 + P(15, 6) * _tmp5);
|
||||
_k(16, 0) = _tmp6 * (-P(16, 22) * _tmp3 - P(16, 23) * _tmp4 + P(16, 4) * _tmp3 +
|
||||
P(16, 5) * _tmp4 + P(16, 6) * _tmp5);
|
||||
_k(17, 0) = _tmp6 * (-P(17, 22) * _tmp3 - P(17, 23) * _tmp4 + P(17, 4) * _tmp3 +
|
||||
P(17, 5) * _tmp4 + P(17, 6) * _tmp5);
|
||||
_k(18, 0) = _tmp6 * (-P(18, 22) * _tmp3 - P(18, 23) * _tmp4 + P(18, 4) * _tmp3 +
|
||||
P(18, 5) * _tmp4 + P(18, 6) * _tmp5);
|
||||
_k(19, 0) = _tmp6 * (-P(19, 22) * _tmp3 - P(19, 23) * _tmp4 + P(19, 4) * _tmp3 +
|
||||
P(19, 5) * _tmp4 + P(19, 6) * _tmp5);
|
||||
_k(20, 0) = _tmp6 * (-P(20, 22) * _tmp3 - P(20, 23) * _tmp4 + P(20, 4) * _tmp3 +
|
||||
P(20, 5) * _tmp4 + P(20, 6) * _tmp5);
|
||||
_k(21, 0) = _tmp6 * (-P(21, 22) * _tmp3 - P(21, 23) * _tmp4 + P(21, 4) * _tmp3 +
|
||||
P(21, 5) * _tmp4 + P(21, 6) * _tmp5);
|
||||
_k(22, 0) = _tmp6 * (-P(22, 22) * _tmp3 - P(22, 23) * _tmp4 + P(22, 4) * _tmp3 +
|
||||
P(22, 5) * _tmp4 + P(22, 6) * _tmp5);
|
||||
_k(23, 0) = _tmp6 * (-P(23, 22) * _tmp3 - P(23, 23) * _tmp4 + P(23, 4) * _tmp3 +
|
||||
P(23, 5) * _tmp4 + P(23, 6) * _tmp5);
|
||||
_k(0, 0) = _tmp6 * (-P(0, 21) * _tmp3 - P(0, 22) * _tmp4 + P(0, 3) * _tmp3 + P(0, 4) * _tmp4 +
|
||||
P(0, 5) * _tmp5);
|
||||
_k(1, 0) = _tmp6 * (-P(1, 21) * _tmp3 - P(1, 22) * _tmp4 + P(1, 3) * _tmp3 + P(1, 4) * _tmp4 +
|
||||
P(1, 5) * _tmp5);
|
||||
_k(2, 0) = _tmp6 * (-P(2, 21) * _tmp3 - P(2, 22) * _tmp4 + P(2, 3) * _tmp3 + P(2, 4) * _tmp4 +
|
||||
P(2, 5) * _tmp5);
|
||||
_k(3, 0) = _tmp6 * (-P(3, 21) * _tmp3 - P(3, 22) * _tmp4 + P(3, 3) * _tmp3 + P(3, 4) * _tmp4 +
|
||||
P(3, 5) * _tmp5);
|
||||
_k(4, 0) = _tmp6 * (-P(4, 21) * _tmp3 - P(4, 22) * _tmp4 + P(4, 3) * _tmp3 + P(4, 4) * _tmp4 +
|
||||
P(4, 5) * _tmp5);
|
||||
_k(5, 0) = _tmp6 * (-P(5, 21) * _tmp3 - P(5, 22) * _tmp4 + P(5, 3) * _tmp3 + P(5, 4) * _tmp4 +
|
||||
P(5, 5) * _tmp5);
|
||||
_k(6, 0) = _tmp6 * (-P(6, 21) * _tmp3 - P(6, 22) * _tmp4 + P(6, 3) * _tmp3 + P(6, 4) * _tmp4 +
|
||||
P(6, 5) * _tmp5);
|
||||
_k(7, 0) = _tmp6 * (-P(7, 21) * _tmp3 - P(7, 22) * _tmp4 + P(7, 3) * _tmp3 + P(7, 4) * _tmp4 +
|
||||
P(7, 5) * _tmp5);
|
||||
_k(8, 0) = _tmp6 * (-P(8, 21) * _tmp3 - P(8, 22) * _tmp4 + P(8, 3) * _tmp3 + P(8, 4) * _tmp4 +
|
||||
P(8, 5) * _tmp5);
|
||||
_k(9, 0) = _tmp6 * (-P(9, 21) * _tmp3 - P(9, 22) * _tmp4 + P(9, 3) * _tmp3 + P(9, 4) * _tmp4 +
|
||||
P(9, 5) * _tmp5);
|
||||
_k(10, 0) = _tmp6 * (-P(10, 21) * _tmp3 - P(10, 22) * _tmp4 + P(10, 3) * _tmp3 +
|
||||
P(10, 4) * _tmp4 + P(10, 5) * _tmp5);
|
||||
_k(11, 0) = _tmp6 * (-P(11, 21) * _tmp3 - P(11, 22) * _tmp4 + P(11, 3) * _tmp3 +
|
||||
P(11, 4) * _tmp4 + P(11, 5) * _tmp5);
|
||||
_k(12, 0) = _tmp6 * (-P(12, 21) * _tmp3 - P(12, 22) * _tmp4 + P(12, 3) * _tmp3 +
|
||||
P(12, 4) * _tmp4 + P(12, 5) * _tmp5);
|
||||
_k(13, 0) = _tmp6 * (-P(13, 21) * _tmp3 - P(13, 22) * _tmp4 + P(13, 3) * _tmp3 +
|
||||
P(13, 4) * _tmp4 + P(13, 5) * _tmp5);
|
||||
_k(14, 0) = _tmp6 * (-P(14, 21) * _tmp3 - P(14, 22) * _tmp4 + P(14, 3) * _tmp3 +
|
||||
P(14, 4) * _tmp4 + P(14, 5) * _tmp5);
|
||||
_k(15, 0) = _tmp6 * (-P(15, 21) * _tmp3 - P(15, 22) * _tmp4 + P(15, 3) * _tmp3 +
|
||||
P(15, 4) * _tmp4 + P(15, 5) * _tmp5);
|
||||
_k(16, 0) = _tmp6 * (-P(16, 21) * _tmp3 - P(16, 22) * _tmp4 + P(16, 3) * _tmp3 +
|
||||
P(16, 4) * _tmp4 + P(16, 5) * _tmp5);
|
||||
_k(17, 0) = _tmp6 * (-P(17, 21) * _tmp3 - P(17, 22) * _tmp4 + P(17, 3) * _tmp3 +
|
||||
P(17, 4) * _tmp4 + P(17, 5) * _tmp5);
|
||||
_k(18, 0) = _tmp6 * (-P(18, 21) * _tmp3 - P(18, 22) * _tmp4 + P(18, 3) * _tmp3 +
|
||||
P(18, 4) * _tmp4 + P(18, 5) * _tmp5);
|
||||
_k(19, 0) = _tmp6 * (-P(19, 21) * _tmp3 - P(19, 22) * _tmp4 + P(19, 3) * _tmp3 +
|
||||
P(19, 4) * _tmp4 + P(19, 5) * _tmp5);
|
||||
_k(20, 0) = _tmp6 * (-P(20, 21) * _tmp3 - P(20, 22) * _tmp4 + P(20, 3) * _tmp3 +
|
||||
P(20, 4) * _tmp4 + P(20, 5) * _tmp5);
|
||||
_k(21, 0) = _tmp6 * (-P(21, 21) * _tmp3 - P(21, 22) * _tmp4 + P(21, 3) * _tmp3 +
|
||||
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);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+12
-12
@@ -17,7 +17,7 @@ namespace sym {
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* P: Matrix23_23
|
||||
* airspeed: Scalar
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
@@ -28,7 +28,7 @@ namespace sym {
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar airspeed,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar airspeed,
|
||||
const Scalar R, const Scalar epsilon,
|
||||
Scalar* const innov = nullptr,
|
||||
Scalar* const innov_var = nullptr) {
|
||||
@@ -57,16 +57,16 @@ void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R +
|
||||
_tmp4 * (-P(22, 6) * _tmp5 - P(23, 6) * _tmp6 + P(4, 6) * _tmp5 + P(5, 6) * _tmp6 +
|
||||
P(6, 6) * _tmp4) -
|
||||
_tmp5 * (-P(22, 22) * _tmp5 - P(23, 22) * _tmp6 + P(4, 22) * _tmp5 +
|
||||
P(5, 22) * _tmp6 + P(6, 22) * _tmp4) +
|
||||
_tmp5 * (-P(22, 4) * _tmp5 - P(23, 4) * _tmp6 + P(4, 4) * _tmp5 + P(5, 4) * _tmp6 +
|
||||
P(6, 4) * _tmp4) -
|
||||
_tmp6 * (-P(22, 23) * _tmp5 - P(23, 23) * _tmp6 + P(4, 23) * _tmp5 +
|
||||
P(5, 23) * _tmp6 + P(6, 23) * _tmp4) +
|
||||
_tmp6 * (-P(22, 5) * _tmp5 - P(23, 5) * _tmp6 + P(4, 5) * _tmp5 + P(5, 5) * _tmp6 +
|
||||
P(6, 5) * _tmp4);
|
||||
_tmp4 * (-P(21, 5) * _tmp5 - P(22, 5) * _tmp6 + P(3, 5) * _tmp5 + P(4, 5) * _tmp6 +
|
||||
P(5, 5) * _tmp4) -
|
||||
_tmp5 * (-P(21, 21) * _tmp5 - P(22, 21) * _tmp6 + P(3, 21) * _tmp5 +
|
||||
P(4, 21) * _tmp6 + P(5, 21) * _tmp4) +
|
||||
_tmp5 * (-P(21, 3) * _tmp5 - P(22, 3) * _tmp6 + P(3, 3) * _tmp5 + P(4, 3) * _tmp6 +
|
||||
P(5, 3) * _tmp4) -
|
||||
_tmp6 * (-P(21, 22) * _tmp5 - P(22, 22) * _tmp6 + P(3, 22) * _tmp5 +
|
||||
P(4, 22) * _tmp6 + P(5, 22) * _tmp4) +
|
||||
_tmp6 * (-P(21, 4) * _tmp5 - P(22, 4) * _tmp6 + P(3, 4) * _tmp5 + P(4, 4) * _tmp6 +
|
||||
P(5, 4) * _tmp4);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+115
-126
@@ -17,7 +17,7 @@ namespace sym {
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* P: Matrix23_23
|
||||
* rho: Scalar
|
||||
* cd: Scalar
|
||||
* cm: Scalar
|
||||
@@ -26,134 +26,123 @@ namespace sym {
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* K: Matrix24_1
|
||||
* K: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar rho,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar rho,
|
||||
const Scalar cd, const Scalar cm, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
|
||||
// Total ops: 398
|
||||
matrix::Matrix<Scalar, 23, 1>* const K = nullptr) {
|
||||
// Total ops: 348
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (77)
|
||||
const Scalar _tmp0 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp1 = 4 * _tmp0;
|
||||
const Scalar _tmp2 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp3 = 2 * state(0, 0);
|
||||
const Scalar _tmp4 = _tmp2 * _tmp3;
|
||||
const Scalar _tmp5 = 2 * state(6, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(1, 0);
|
||||
const Scalar _tmp7 = -_tmp1 * state(3, 0) + _tmp4 + _tmp6;
|
||||
const Scalar _tmp8 = 2 * state(2, 0);
|
||||
const Scalar _tmp9 = _tmp2 * _tmp8;
|
||||
const Scalar _tmp10 = 2 * state(1, 0);
|
||||
const Scalar _tmp11 = _tmp0 * _tmp10;
|
||||
const Scalar _tmp12 = _tmp8 * state(3, 0);
|
||||
const Scalar _tmp13 = _tmp3 * state(1, 0);
|
||||
const Scalar _tmp14 = _tmp12 - _tmp13;
|
||||
const Scalar _tmp15 = _tmp8 * state(0, 0);
|
||||
const Scalar _tmp16 = _tmp10 * state(3, 0);
|
||||
const Scalar _tmp17 = _tmp15 + _tmp16;
|
||||
const Scalar _tmp18 = -2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp19 = 1 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp20 = _tmp18 + _tmp19;
|
||||
const Scalar _tmp21 = _tmp0 * _tmp17 + _tmp14 * _tmp2 + _tmp20 * state(6, 0);
|
||||
const Scalar _tmp22 = 2 * _tmp21;
|
||||
const Scalar _tmp23 = _tmp0 * _tmp3;
|
||||
const Scalar _tmp24 = 4 * _tmp2;
|
||||
const Scalar _tmp25 = _tmp8 * state(6, 0);
|
||||
const Scalar _tmp26 = -2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp27 = _tmp19 + _tmp26;
|
||||
const Scalar _tmp28 = _tmp3 * state(3, 0);
|
||||
const Scalar _tmp29 = _tmp8 * state(1, 0);
|
||||
const Scalar _tmp30 = -_tmp28 + _tmp29;
|
||||
const Scalar _tmp31 = _tmp12 + _tmp13;
|
||||
const Scalar _tmp32 = _tmp0 * _tmp30 + _tmp2 * _tmp27 + _tmp31 * state(6, 0);
|
||||
const Scalar _tmp33 = 2 * _tmp32;
|
||||
const Scalar _tmp34 = _tmp18 + _tmp26 + 1;
|
||||
const Scalar _tmp35 = _tmp28 + _tmp29;
|
||||
const Scalar _tmp36 = -_tmp15 + _tmp16;
|
||||
const Scalar _tmp37 = _tmp0 * _tmp34 + _tmp2 * _tmp35 + _tmp36 * state(6, 0);
|
||||
const Scalar _tmp38 = 2 * _tmp37;
|
||||
const Scalar _tmp39 = std::sqrt(Scalar(std::pow(_tmp21, Scalar(2)) + std::pow(_tmp32, Scalar(2)) +
|
||||
std::pow(_tmp37, Scalar(2)) + epsilon));
|
||||
const Scalar _tmp40 = cd * rho;
|
||||
const Scalar _tmp41 = Scalar(0.25) * _tmp37 * _tmp40 / _tmp39;
|
||||
const Scalar _tmp42 = Scalar(0.5) * _tmp39 * _tmp40;
|
||||
const Scalar _tmp43 =
|
||||
-_tmp41 * (_tmp22 * (_tmp11 + _tmp9) + _tmp33 * (-_tmp23 - _tmp24 * state(3, 0) + _tmp25) +
|
||||
_tmp38 * _tmp7) -
|
||||
_tmp42 * _tmp7 - _tmp7 * cm;
|
||||
const Scalar _tmp44 = _tmp35 * cm;
|
||||
const Scalar _tmp45 = _tmp35 * _tmp38;
|
||||
const Scalar _tmp46 = _tmp14 * _tmp22;
|
||||
const Scalar _tmp47 = _tmp27 * _tmp33;
|
||||
const Scalar _tmp48 = _tmp35 * _tmp42;
|
||||
const Scalar _tmp49 = -_tmp41 * (-_tmp45 - _tmp46 - _tmp47) + _tmp44 + _tmp48;
|
||||
const Scalar _tmp50 = -_tmp41 * (_tmp45 + _tmp46 + _tmp47) - _tmp44 - _tmp48;
|
||||
const Scalar _tmp51 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp52 = _tmp51 + _tmp9;
|
||||
const Scalar _tmp53 = 2 * state(3, 0);
|
||||
const Scalar _tmp54 = _tmp0 * _tmp53;
|
||||
const Scalar _tmp55 = 4 * state(6, 0);
|
||||
const Scalar _tmp56 = _tmp0 * _tmp8;
|
||||
const Scalar _tmp57 = _tmp3 * state(6, 0);
|
||||
const Scalar _tmp58 =
|
||||
-_tmp41 * (_tmp22 * (-_tmp4 + _tmp54 - _tmp55 * state(1, 0)) +
|
||||
_tmp33 * (-_tmp24 * state(1, 0) + _tmp56 + _tmp57) + _tmp38 * _tmp52) -
|
||||
_tmp42 * _tmp52 - _tmp52 * cm;
|
||||
const Scalar _tmp59 = _tmp10 * _tmp2;
|
||||
const Scalar _tmp60 = -_tmp1 * state(2, 0) - _tmp57 + _tmp59;
|
||||
const Scalar _tmp61 = _tmp2 * _tmp53;
|
||||
const Scalar _tmp62 = -_tmp41 * (_tmp22 * (_tmp23 - _tmp55 * state(2, 0) + _tmp61) +
|
||||
_tmp33 * (_tmp11 + _tmp51) + _tmp38 * _tmp60) -
|
||||
_tmp42 * _tmp60 - _tmp60 * cm;
|
||||
const Scalar _tmp63 = _tmp34 * cm;
|
||||
const Scalar _tmp64 = _tmp34 * _tmp38;
|
||||
const Scalar _tmp65 = _tmp17 * _tmp22;
|
||||
const Scalar _tmp66 = _tmp30 * _tmp33;
|
||||
const Scalar _tmp67 = _tmp34 * _tmp42;
|
||||
const Scalar _tmp68 = -_tmp41 * (-_tmp64 - _tmp65 - _tmp66) + _tmp63 + _tmp67;
|
||||
const Scalar _tmp69 = -_tmp25 + _tmp61;
|
||||
const Scalar _tmp0 = 2 * state(0, 0);
|
||||
const Scalar _tmp1 = _tmp0 * state(3, 0);
|
||||
const Scalar _tmp2 = 2 * state(2, 0);
|
||||
const Scalar _tmp3 = _tmp2 * state(1, 0);
|
||||
const Scalar _tmp4 = _tmp1 + _tmp3;
|
||||
const Scalar _tmp5 = _tmp4 * cm;
|
||||
const Scalar _tmp6 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp7 = -2 * _tmp6;
|
||||
const Scalar _tmp8 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp9 = -2 * _tmp8;
|
||||
const Scalar _tmp10 = _tmp7 + _tmp9 + 1;
|
||||
const Scalar _tmp11 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp12 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp13 = _tmp2 * state(0, 0);
|
||||
const Scalar _tmp14 = -_tmp13;
|
||||
const Scalar _tmp15 = 2 * state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp16 = _tmp14 + _tmp15;
|
||||
const Scalar _tmp17 = _tmp12 * _tmp4 + _tmp16 * state(6, 0);
|
||||
const Scalar _tmp18 = _tmp10 * _tmp11 + _tmp17;
|
||||
const Scalar _tmp19 = 2 * _tmp18;
|
||||
const Scalar _tmp20 = _tmp19 * _tmp4;
|
||||
const Scalar _tmp21 = _tmp2 * state(3, 0);
|
||||
const Scalar _tmp22 = _tmp0 * state(1, 0);
|
||||
const Scalar _tmp23 = -_tmp22;
|
||||
const Scalar _tmp24 = _tmp21 + _tmp23;
|
||||
const Scalar _tmp25 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp26 = 1 - 2 * _tmp25;
|
||||
const Scalar _tmp27 = _tmp26 + _tmp9;
|
||||
const Scalar _tmp28 = _tmp13 + _tmp15;
|
||||
const Scalar _tmp29 = _tmp11 * _tmp28 + _tmp12 * _tmp24;
|
||||
const Scalar _tmp30 = _tmp27 * state(6, 0) + _tmp29;
|
||||
const Scalar _tmp31 = 2 * _tmp30;
|
||||
const Scalar _tmp32 = _tmp24 * _tmp31;
|
||||
const Scalar _tmp33 = _tmp26 + _tmp7;
|
||||
const Scalar _tmp34 = -_tmp1;
|
||||
const Scalar _tmp35 = _tmp3 + _tmp34;
|
||||
const Scalar _tmp36 = _tmp21 + _tmp22;
|
||||
const Scalar _tmp37 = _tmp11 * _tmp35 + _tmp36 * state(6, 0);
|
||||
const Scalar _tmp38 = _tmp12 * _tmp33 + _tmp37;
|
||||
const Scalar _tmp39 = 2 * _tmp38;
|
||||
const Scalar _tmp40 = _tmp33 * _tmp39;
|
||||
const Scalar _tmp41 = std::sqrt(Scalar(std::pow(_tmp18, Scalar(2)) + std::pow(_tmp30, Scalar(2)) +
|
||||
std::pow(_tmp38, Scalar(2)) + epsilon));
|
||||
const Scalar _tmp42 = cd * rho;
|
||||
const Scalar _tmp43 = Scalar(0.25) * _tmp18 * _tmp42 / _tmp41;
|
||||
const Scalar _tmp44 = Scalar(0.5) * _tmp41 * _tmp42;
|
||||
const Scalar _tmp45 = _tmp4 * _tmp44;
|
||||
const Scalar _tmp46 = -_tmp43 * (_tmp20 + _tmp32 + _tmp40) - _tmp45 - _tmp5;
|
||||
const Scalar _tmp47 = -_tmp25;
|
||||
const Scalar _tmp48 = _tmp47 + _tmp6;
|
||||
const Scalar _tmp49 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp50 = -_tmp49;
|
||||
const Scalar _tmp51 = _tmp50 + _tmp8;
|
||||
const Scalar _tmp52 = -_tmp3;
|
||||
const Scalar _tmp53 = -_tmp15;
|
||||
const Scalar _tmp54 = -_tmp6;
|
||||
const Scalar _tmp55 = _tmp12 * (_tmp47 + _tmp49 + _tmp54 + _tmp8) + _tmp37;
|
||||
const Scalar _tmp56 = -_tmp43 * (_tmp19 * _tmp55 + _tmp39 * (_tmp11 * (_tmp48 + _tmp51) +
|
||||
_tmp12 * (_tmp34 + _tmp52) +
|
||||
state(6, 0) * (_tmp13 + _tmp53))) -
|
||||
_tmp44 * _tmp55 - _tmp55 * cm;
|
||||
const Scalar _tmp57 = -_tmp43 * (-_tmp20 - _tmp32 - _tmp40) + _tmp45 + _tmp5;
|
||||
const Scalar _tmp58 = _tmp10 * cm;
|
||||
const Scalar _tmp59 = _tmp10 * _tmp19;
|
||||
const Scalar _tmp60 = _tmp28 * _tmp31;
|
||||
const Scalar _tmp61 = _tmp35 * _tmp39;
|
||||
const Scalar _tmp62 = _tmp10 * _tmp44;
|
||||
const Scalar _tmp63 = -_tmp43 * (-_tmp59 - _tmp60 - _tmp61) + _tmp58 + _tmp62;
|
||||
const Scalar _tmp64 = -_tmp8;
|
||||
const Scalar _tmp65 = -_tmp21;
|
||||
const Scalar _tmp66 = _tmp49 + _tmp64;
|
||||
const Scalar _tmp67 =
|
||||
_tmp43 * (_tmp31 * (_tmp11 * (_tmp1 + _tmp52) + _tmp12 * (_tmp25 + _tmp50 + _tmp6 + _tmp64) +
|
||||
state(6, 0) * (_tmp23 + _tmp65)) +
|
||||
_tmp39 * (_tmp29 + state(6, 0) * (_tmp48 + _tmp66)));
|
||||
const Scalar _tmp68 = _tmp25 + _tmp54;
|
||||
const Scalar _tmp69 =
|
||||
_tmp11 * (_tmp14 + _tmp53) + _tmp12 * (_tmp22 + _tmp65) + state(6, 0) * (_tmp51 + _tmp68);
|
||||
const Scalar _tmp70 =
|
||||
-_tmp41 * (_tmp22 * (_tmp56 - _tmp59) + _tmp33 * (-_tmp54 + _tmp6) + _tmp38 * _tmp69) -
|
||||
_tmp42 * _tmp69 - _tmp69 * cm;
|
||||
const Scalar _tmp71 = -_tmp41 * (_tmp64 + _tmp65 + _tmp66) - _tmp63 - _tmp67;
|
||||
const Scalar _tmp72 = -_tmp36 * _tmp42 - _tmp36 * cm -
|
||||
_tmp41 * (_tmp20 * _tmp22 + _tmp31 * _tmp33 + _tmp36 * _tmp38);
|
||||
const Scalar _tmp73 = P(23, 23) * _tmp49;
|
||||
const Scalar _tmp74 = P(22, 22) * _tmp68;
|
||||
const Scalar _tmp75 = R +
|
||||
_tmp43 * (P(0, 3) * _tmp70 + P(1, 3) * _tmp58 + P(2, 3) * _tmp62 +
|
||||
P(22, 3) * _tmp68 + P(23, 3) * _tmp49 + P(3, 3) * _tmp43 +
|
||||
P(4, 3) * _tmp71 + P(5, 3) * _tmp50 + P(6, 3) * _tmp72) +
|
||||
_tmp49 * (P(0, 23) * _tmp70 + P(1, 23) * _tmp58 + P(2, 23) * _tmp62 +
|
||||
P(22, 23) * _tmp68 + P(3, 23) * _tmp43 + P(4, 23) * _tmp71 +
|
||||
P(5, 23) * _tmp50 + P(6, 23) * _tmp72 + _tmp73) +
|
||||
_tmp50 * (P(0, 5) * _tmp70 + P(1, 5) * _tmp58 + P(2, 5) * _tmp62 +
|
||||
P(22, 5) * _tmp68 + P(23, 5) * _tmp49 + P(3, 5) * _tmp43 +
|
||||
P(4, 5) * _tmp71 + P(5, 5) * _tmp50 + P(6, 5) * _tmp72) +
|
||||
_tmp58 * (P(0, 1) * _tmp70 + P(1, 1) * _tmp58 + P(2, 1) * _tmp62 +
|
||||
P(22, 1) * _tmp68 + P(23, 1) * _tmp49 + P(3, 1) * _tmp43 +
|
||||
P(4, 1) * _tmp71 + P(5, 1) * _tmp50 + P(6, 1) * _tmp72) +
|
||||
_tmp62 * (P(0, 2) * _tmp70 + P(1, 2) * _tmp58 + P(2, 2) * _tmp62 +
|
||||
P(22, 2) * _tmp68 + P(23, 2) * _tmp49 + P(3, 2) * _tmp43 +
|
||||
P(4, 2) * _tmp71 + P(5, 2) * _tmp50 + P(6, 2) * _tmp72) +
|
||||
_tmp68 * (P(0, 22) * _tmp70 + P(1, 22) * _tmp58 + P(2, 22) * _tmp62 +
|
||||
P(23, 22) * _tmp49 + P(3, 22) * _tmp43 + P(4, 22) * _tmp71 +
|
||||
P(5, 22) * _tmp50 + P(6, 22) * _tmp72 + _tmp74) +
|
||||
_tmp70 * (P(0, 0) * _tmp70 + P(1, 0) * _tmp58 + P(2, 0) * _tmp62 +
|
||||
P(22, 0) * _tmp68 + P(23, 0) * _tmp49 + P(3, 0) * _tmp43 +
|
||||
P(4, 0) * _tmp71 + P(5, 0) * _tmp50 + P(6, 0) * _tmp72) +
|
||||
_tmp71 * (P(0, 4) * _tmp70 + P(1, 4) * _tmp58 + P(2, 4) * _tmp62 +
|
||||
P(22, 4) * _tmp68 + P(23, 4) * _tmp49 + P(3, 4) * _tmp43 +
|
||||
P(4, 4) * _tmp71 + P(5, 4) * _tmp50 + P(6, 4) * _tmp72) +
|
||||
_tmp72 * (P(0, 6) * _tmp70 + P(1, 6) * _tmp58 + P(2, 6) * _tmp62 +
|
||||
P(22, 6) * _tmp68 + P(23, 6) * _tmp49 + P(3, 6) * _tmp43 +
|
||||
P(4, 6) * _tmp71 + P(5, 6) * _tmp50 + P(6, 6) * _tmp72);
|
||||
-_tmp43 * (_tmp19 * _tmp69 + _tmp31 * (_tmp11 * (_tmp66 + _tmp68) + _tmp17)) -
|
||||
_tmp44 * _tmp69 - _tmp69 * cm;
|
||||
const Scalar _tmp71 = -_tmp43 * (_tmp59 + _tmp60 + _tmp61) - _tmp58 - _tmp62;
|
||||
const Scalar _tmp72 = -_tmp16 * _tmp44 - _tmp16 * cm -
|
||||
_tmp43 * (_tmp16 * _tmp19 + _tmp27 * _tmp31 + _tmp36 * _tmp39);
|
||||
const Scalar _tmp73 = P(21, 21) * _tmp63;
|
||||
const Scalar _tmp74 = P(22, 22) * _tmp57;
|
||||
const Scalar _tmp75 =
|
||||
R +
|
||||
_tmp46 * (-P(0, 4) * _tmp67 + P(1, 4) * _tmp70 + P(2, 4) * _tmp56 + P(21, 4) * _tmp63 +
|
||||
P(22, 4) * _tmp57 + P(3, 4) * _tmp71 + P(4, 4) * _tmp46 + P(5, 4) * _tmp72) +
|
||||
_tmp56 * (-P(0, 2) * _tmp67 + P(1, 2) * _tmp70 + P(2, 2) * _tmp56 + P(21, 2) * _tmp63 +
|
||||
P(22, 2) * _tmp57 + P(3, 2) * _tmp71 + P(4, 2) * _tmp46 + P(5, 2) * _tmp72) +
|
||||
_tmp57 * (-P(0, 22) * _tmp67 + P(1, 22) * _tmp70 + P(2, 22) * _tmp56 + P(21, 22) * _tmp63 +
|
||||
P(3, 22) * _tmp71 + P(4, 22) * _tmp46 + P(5, 22) * _tmp72 + _tmp74) +
|
||||
_tmp63 * (-P(0, 21) * _tmp67 + P(1, 21) * _tmp70 + P(2, 21) * _tmp56 + P(22, 21) * _tmp57 +
|
||||
P(3, 21) * _tmp71 + P(4, 21) * _tmp46 + P(5, 21) * _tmp72 + _tmp73) -
|
||||
_tmp67 * (-P(0, 0) * _tmp67 + P(1, 0) * _tmp70 + P(2, 0) * _tmp56 + P(21, 0) * _tmp63 +
|
||||
P(22, 0) * _tmp57 + P(3, 0) * _tmp71 + P(4, 0) * _tmp46 + P(5, 0) * _tmp72) +
|
||||
_tmp70 * (-P(0, 1) * _tmp67 + P(1, 1) * _tmp70 + P(2, 1) * _tmp56 + P(21, 1) * _tmp63 +
|
||||
P(22, 1) * _tmp57 + P(3, 1) * _tmp71 + P(4, 1) * _tmp46 + P(5, 1) * _tmp72) +
|
||||
_tmp71 * (-P(0, 3) * _tmp67 + P(1, 3) * _tmp70 + P(2, 3) * _tmp56 + P(21, 3) * _tmp63 +
|
||||
P(22, 3) * _tmp57 + P(3, 3) * _tmp71 + P(4, 3) * _tmp46 + P(5, 3) * _tmp72) +
|
||||
_tmp72 * (-P(0, 5) * _tmp67 + P(1, 5) * _tmp70 + P(2, 5) * _tmp56 + P(21, 5) * _tmp63 +
|
||||
P(22, 5) * _tmp57 + P(3, 5) * _tmp71 + P(4, 5) * _tmp46 + P(5, 5) * _tmp72);
|
||||
const Scalar _tmp76 = Scalar(1.0) / (math::max<Scalar>(_tmp75, epsilon));
|
||||
|
||||
// Output terms (2)
|
||||
@@ -164,16 +153,16 @@ void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
|
||||
matrix::Matrix<Scalar, 23, 1>& _k = (*K);
|
||||
|
||||
_k.setZero();
|
||||
|
||||
_k(22, 0) = _tmp76 * (P(22, 0) * _tmp70 + P(22, 1) * _tmp58 + P(22, 2) * _tmp62 +
|
||||
P(22, 23) * _tmp49 + P(22, 3) * _tmp43 + P(22, 4) * _tmp71 +
|
||||
P(22, 5) * _tmp50 + P(22, 6) * _tmp72 + _tmp74);
|
||||
_k(23, 0) = _tmp76 * (P(23, 0) * _tmp70 + P(23, 1) * _tmp58 + P(23, 2) * _tmp62 +
|
||||
P(23, 22) * _tmp68 + P(23, 3) * _tmp43 + P(23, 4) * _tmp71 +
|
||||
P(23, 5) * _tmp50 + P(23, 6) * _tmp72 + _tmp73);
|
||||
_k(21, 0) =
|
||||
_tmp76 * (-P(21, 0) * _tmp67 + P(21, 1) * _tmp70 + P(21, 2) * _tmp56 + P(21, 22) * _tmp57 +
|
||||
P(21, 3) * _tmp71 + P(21, 4) * _tmp46 + P(21, 5) * _tmp72 + _tmp73);
|
||||
_k(22, 0) =
|
||||
_tmp76 * (-P(22, 0) * _tmp67 + P(22, 1) * _tmp70 + P(22, 2) * _tmp56 + P(22, 21) * _tmp63 +
|
||||
P(22, 3) * _tmp71 + P(22, 4) * _tmp46 + P(22, 5) * _tmp72 + _tmp74);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+119
-131
@@ -17,7 +17,7 @@ namespace sym {
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* P: Matrix23_23
|
||||
* rho: Scalar
|
||||
* cd: Scalar
|
||||
* cm: Scalar
|
||||
@@ -26,157 +26,145 @@ namespace sym {
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* K: Matrix24_1
|
||||
* K: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeDragYInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar rho,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar rho,
|
||||
const Scalar cd, const Scalar cm, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
|
||||
// Total ops: 397
|
||||
matrix::Matrix<Scalar, 23, 1>* const K = nullptr) {
|
||||
// Total ops: 348
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (76)
|
||||
const Scalar _tmp0 = 2 * state(0, 0);
|
||||
const Scalar _tmp1 = _tmp0 * state(3, 0);
|
||||
const Scalar _tmp2 = 2 * state(1, 0);
|
||||
const Scalar _tmp3 = _tmp2 * state(2, 0);
|
||||
const Scalar _tmp4 = -_tmp1 + _tmp3;
|
||||
const Scalar _tmp5 = _tmp4 * cm;
|
||||
const Scalar _tmp6 = -2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp7 = -2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp8 = _tmp6 + _tmp7 + 1;
|
||||
// Intermediate terms (77)
|
||||
const Scalar _tmp0 = 2 * state(2, 0) * state(3, 0);
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 = _tmp1 * state(0, 0);
|
||||
const Scalar _tmp3 = _tmp0 + _tmp2;
|
||||
const Scalar _tmp4 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp5 = -2 * _tmp4;
|
||||
const Scalar _tmp6 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp7 = -2 * _tmp6;
|
||||
const Scalar _tmp8 = _tmp5 + _tmp7 + 1;
|
||||
const Scalar _tmp9 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp10 = _tmp1 + _tmp3;
|
||||
const Scalar _tmp11 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp12 = _tmp0 * state(2, 0);
|
||||
const Scalar _tmp13 = _tmp2 * state(3, 0);
|
||||
const Scalar _tmp14 = -_tmp12 + _tmp13;
|
||||
const Scalar _tmp15 = _tmp10 * _tmp11 + _tmp14 * state(6, 0) + _tmp8 * _tmp9;
|
||||
const Scalar _tmp16 = 2 * state(2, 0);
|
||||
const Scalar _tmp17 = _tmp16 * state(3, 0);
|
||||
const Scalar _tmp18 = _tmp2 * state(0, 0);
|
||||
const Scalar _tmp19 = _tmp17 - _tmp18;
|
||||
const Scalar _tmp20 = _tmp12 + _tmp13;
|
||||
const Scalar _tmp21 = 1 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp22 = _tmp21 + _tmp7;
|
||||
const Scalar _tmp23 = _tmp11 * _tmp19 + _tmp20 * _tmp9 + _tmp22 * state(6, 0);
|
||||
const Scalar _tmp24 = _tmp21 + _tmp6;
|
||||
const Scalar _tmp25 = _tmp17 + _tmp18;
|
||||
const Scalar _tmp26 = _tmp11 * _tmp24 + _tmp25 * state(6, 0) + _tmp4 * _tmp9;
|
||||
const Scalar _tmp27 = std::sqrt(Scalar(std::pow(_tmp15, Scalar(2)) + std::pow(_tmp23, Scalar(2)) +
|
||||
std::pow(_tmp26, Scalar(2)) + epsilon));
|
||||
const Scalar _tmp28 = cd * rho;
|
||||
const Scalar _tmp29 = Scalar(0.5) * _tmp27 * _tmp28;
|
||||
const Scalar _tmp30 = _tmp29 * _tmp4;
|
||||
const Scalar _tmp31 = 2 * _tmp15;
|
||||
const Scalar _tmp32 = _tmp31 * _tmp8;
|
||||
const Scalar _tmp33 = 2 * _tmp23;
|
||||
const Scalar _tmp34 = _tmp20 * _tmp33;
|
||||
const Scalar _tmp35 = 2 * _tmp26;
|
||||
const Scalar _tmp36 = _tmp35 * _tmp4;
|
||||
const Scalar _tmp37 = Scalar(0.25) * _tmp26 * _tmp28 / _tmp27;
|
||||
const Scalar _tmp38 = -_tmp30 - _tmp37 * (_tmp32 + _tmp34 + _tmp36) - _tmp5;
|
||||
const Scalar _tmp39 = -_tmp25 * _tmp29 - _tmp25 * cm -
|
||||
_tmp37 * (_tmp14 * _tmp31 + _tmp22 * _tmp33 + _tmp25 * _tmp35);
|
||||
const Scalar _tmp40 = 2 * state(3, 0);
|
||||
const Scalar _tmp41 = _tmp40 * _tmp9;
|
||||
const Scalar _tmp42 = _tmp2 * state(6, 0);
|
||||
const Scalar _tmp43 = -_tmp41 + _tmp42;
|
||||
const Scalar _tmp44 = _tmp11 * _tmp2;
|
||||
const Scalar _tmp45 = _tmp16 * _tmp9;
|
||||
const Scalar _tmp46 = _tmp11 * _tmp40;
|
||||
const Scalar _tmp47 = _tmp16 * state(6, 0);
|
||||
const Scalar _tmp48 =
|
||||
-_tmp29 * _tmp43 -
|
||||
_tmp37 * (_tmp31 * (_tmp46 - _tmp47) + _tmp33 * (-_tmp44 + _tmp45) + _tmp35 * _tmp43) -
|
||||
_tmp43 * cm;
|
||||
const Scalar _tmp49 = _tmp2 * _tmp9;
|
||||
const Scalar _tmp50 = _tmp40 * state(6, 0);
|
||||
const Scalar _tmp51 = _tmp49 + _tmp50;
|
||||
const Scalar _tmp52 = _tmp0 * _tmp9;
|
||||
const Scalar _tmp53 = 4 * state(6, 0);
|
||||
const Scalar _tmp54 = 4 * _tmp9;
|
||||
const Scalar _tmp55 = _tmp0 * state(6, 0);
|
||||
const Scalar _tmp10 = 2 * state(0, 0);
|
||||
const Scalar _tmp11 = _tmp10 * state(3, 0);
|
||||
const Scalar _tmp12 = _tmp1 * state(2, 0);
|
||||
const Scalar _tmp13 = _tmp11 + _tmp12;
|
||||
const Scalar _tmp14 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp15 = _tmp10 * state(2, 0);
|
||||
const Scalar _tmp16 = -_tmp15;
|
||||
const Scalar _tmp17 = _tmp1 * state(3, 0);
|
||||
const Scalar _tmp18 = _tmp16 + _tmp17;
|
||||
const Scalar _tmp19 = _tmp13 * _tmp14 + _tmp18 * state(6, 0);
|
||||
const Scalar _tmp20 = _tmp19 + _tmp8 * _tmp9;
|
||||
const Scalar _tmp21 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp22 = 1 - 2 * _tmp21;
|
||||
const Scalar _tmp23 = _tmp22 + _tmp7;
|
||||
const Scalar _tmp24 = -_tmp2;
|
||||
const Scalar _tmp25 = _tmp0 + _tmp24;
|
||||
const Scalar _tmp26 = _tmp15 + _tmp17;
|
||||
const Scalar _tmp27 = _tmp14 * _tmp25 + _tmp26 * _tmp9;
|
||||
const Scalar _tmp28 = _tmp23 * state(6, 0) + _tmp27;
|
||||
const Scalar _tmp29 = _tmp22 + _tmp5;
|
||||
const Scalar _tmp30 = -_tmp11;
|
||||
const Scalar _tmp31 = _tmp12 + _tmp30;
|
||||
const Scalar _tmp32 = _tmp3 * state(6, 0) + _tmp31 * _tmp9;
|
||||
const Scalar _tmp33 = _tmp14 * _tmp29 + _tmp32;
|
||||
const Scalar _tmp34 = std::sqrt(Scalar(std::pow(_tmp20, Scalar(2)) + std::pow(_tmp28, Scalar(2)) +
|
||||
std::pow(_tmp33, Scalar(2)) + epsilon));
|
||||
const Scalar _tmp35 = cd * rho;
|
||||
const Scalar _tmp36 = Scalar(0.5) * _tmp34 * _tmp35;
|
||||
const Scalar _tmp37 = 2 * _tmp20;
|
||||
const Scalar _tmp38 = 2 * _tmp28;
|
||||
const Scalar _tmp39 = 2 * _tmp33;
|
||||
const Scalar _tmp40 = Scalar(0.25) * _tmp33 * _tmp35 / _tmp34;
|
||||
const Scalar _tmp41 =
|
||||
-_tmp3 * _tmp36 - _tmp3 * cm - _tmp40 * (_tmp18 * _tmp37 + _tmp23 * _tmp38 + _tmp3 * _tmp39);
|
||||
const Scalar _tmp42 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp43 = -_tmp42;
|
||||
const Scalar _tmp44 = -_tmp6;
|
||||
const Scalar _tmp45 = -_tmp12;
|
||||
const Scalar _tmp46 = -_tmp0;
|
||||
const Scalar _tmp47 = -_tmp21;
|
||||
const Scalar _tmp48 = _tmp4 + _tmp47;
|
||||
const Scalar _tmp49 = _tmp42 + _tmp44;
|
||||
const Scalar _tmp50 = _tmp27 + state(6, 0) * (_tmp48 + _tmp49);
|
||||
const Scalar _tmp51 =
|
||||
-_tmp36 * _tmp50 -
|
||||
_tmp40 * (_tmp38 * (_tmp14 * (_tmp21 + _tmp4 + _tmp43 + _tmp44) + _tmp9 * (_tmp11 + _tmp45) +
|
||||
state(6, 0) * (_tmp24 + _tmp46)) +
|
||||
_tmp39 * _tmp50) -
|
||||
_tmp50 * cm;
|
||||
const Scalar _tmp52 = _tmp43 + _tmp6;
|
||||
const Scalar _tmp53 = -_tmp17;
|
||||
const Scalar _tmp54 =
|
||||
_tmp14 * (_tmp30 + _tmp45) + _tmp9 * (_tmp48 + _tmp52) + state(6, 0) * (_tmp15 + _tmp53);
|
||||
const Scalar _tmp55 = -_tmp4;
|
||||
const Scalar _tmp56 =
|
||||
-_tmp29 * _tmp51 -
|
||||
_tmp37 * (_tmp31 * (_tmp44 - _tmp54 * state(2, 0) - _tmp55) +
|
||||
_tmp33 * (_tmp46 + _tmp52 - _tmp53 * state(2, 0)) + _tmp35 * _tmp51) -
|
||||
_tmp51 * cm;
|
||||
const Scalar _tmp57 = _tmp24 * cm;
|
||||
const Scalar _tmp58 = _tmp10 * _tmp31;
|
||||
const Scalar _tmp59 = _tmp19 * _tmp33;
|
||||
const Scalar _tmp60 = _tmp24 * _tmp35;
|
||||
const Scalar _tmp61 = _tmp24 * _tmp29;
|
||||
const Scalar _tmp62 = -_tmp37 * (-_tmp58 - _tmp59 - _tmp60) + _tmp57 + _tmp61;
|
||||
const Scalar _tmp63 = _tmp0 * _tmp11;
|
||||
const Scalar _tmp64 = _tmp11 * _tmp16;
|
||||
const Scalar _tmp65 = 4 * _tmp11;
|
||||
const Scalar _tmp66 = _tmp45 + _tmp55 - _tmp65 * state(1, 0);
|
||||
const Scalar _tmp67 =
|
||||
-_tmp29 * _tmp66 -
|
||||
_tmp37 * (_tmp31 * (_tmp50 + _tmp64) + _tmp33 * (_tmp41 - _tmp53 * state(1, 0) - _tmp63) +
|
||||
_tmp35 * _tmp66) -
|
||||
_tmp66 * cm;
|
||||
const Scalar _tmp68 = -_tmp37 * (_tmp58 + _tmp59 + _tmp60) - _tmp57 - _tmp61;
|
||||
const Scalar _tmp69 = _tmp47 - _tmp52 - _tmp65 * state(3, 0);
|
||||
const Scalar _tmp70 = -_tmp29 * _tmp69 -
|
||||
_tmp37 * (_tmp31 * (_tmp42 - _tmp54 * state(3, 0) + _tmp63) +
|
||||
_tmp33 * (_tmp49 + _tmp64) + _tmp35 * _tmp69) -
|
||||
_tmp69 * cm;
|
||||
const Scalar _tmp71 = _tmp30 - _tmp37 * (-_tmp32 - _tmp34 - _tmp36) + _tmp5;
|
||||
const Scalar _tmp72 = P(22, 22) * _tmp71;
|
||||
const Scalar _tmp73 = P(23, 23) * _tmp62;
|
||||
const Scalar _tmp74 = R +
|
||||
_tmp38 * (P(0, 4) * _tmp48 + P(1, 4) * _tmp67 + P(2, 4) * _tmp56 +
|
||||
P(22, 4) * _tmp71 + P(23, 4) * _tmp62 + P(3, 4) * _tmp70 +
|
||||
P(4, 4) * _tmp38 + P(5, 4) * _tmp68 + P(6, 4) * _tmp39) +
|
||||
_tmp39 * (P(0, 6) * _tmp48 + P(1, 6) * _tmp67 + P(2, 6) * _tmp56 +
|
||||
P(22, 6) * _tmp71 + P(23, 6) * _tmp62 + P(3, 6) * _tmp70 +
|
||||
P(4, 6) * _tmp38 + P(5, 6) * _tmp68 + P(6, 6) * _tmp39) +
|
||||
_tmp48 * (P(0, 0) * _tmp48 + P(1, 0) * _tmp67 + P(2, 0) * _tmp56 +
|
||||
P(22, 0) * _tmp71 + P(23, 0) * _tmp62 + P(3, 0) * _tmp70 +
|
||||
P(4, 0) * _tmp38 + P(5, 0) * _tmp68 + P(6, 0) * _tmp39) +
|
||||
_tmp56 * (P(0, 2) * _tmp48 + P(1, 2) * _tmp67 + P(2, 2) * _tmp56 +
|
||||
P(22, 2) * _tmp71 + P(23, 2) * _tmp62 + P(3, 2) * _tmp70 +
|
||||
P(4, 2) * _tmp38 + P(5, 2) * _tmp68 + P(6, 2) * _tmp39) +
|
||||
_tmp62 * (P(0, 23) * _tmp48 + P(1, 23) * _tmp67 + P(2, 23) * _tmp56 +
|
||||
P(22, 23) * _tmp71 + P(3, 23) * _tmp70 + P(4, 23) * _tmp38 +
|
||||
P(5, 23) * _tmp68 + P(6, 23) * _tmp39 + _tmp73) +
|
||||
_tmp67 * (P(0, 1) * _tmp48 + P(1, 1) * _tmp67 + P(2, 1) * _tmp56 +
|
||||
P(22, 1) * _tmp71 + P(23, 1) * _tmp62 + P(3, 1) * _tmp70 +
|
||||
P(4, 1) * _tmp38 + P(5, 1) * _tmp68 + P(6, 1) * _tmp39) +
|
||||
_tmp68 * (P(0, 5) * _tmp48 + P(1, 5) * _tmp67 + P(2, 5) * _tmp56 +
|
||||
P(22, 5) * _tmp71 + P(23, 5) * _tmp62 + P(3, 5) * _tmp70 +
|
||||
P(4, 5) * _tmp38 + P(5, 5) * _tmp68 + P(6, 5) * _tmp39) +
|
||||
_tmp70 * (P(0, 3) * _tmp48 + P(1, 3) * _tmp67 + P(2, 3) * _tmp56 +
|
||||
P(22, 3) * _tmp71 + P(23, 3) * _tmp62 + P(3, 3) * _tmp70 +
|
||||
P(4, 3) * _tmp38 + P(5, 3) * _tmp68 + P(6, 3) * _tmp39) +
|
||||
_tmp71 * (P(0, 22) * _tmp48 + P(1, 22) * _tmp67 + P(2, 22) * _tmp56 +
|
||||
P(23, 22) * _tmp62 + P(3, 22) * _tmp70 + P(4, 22) * _tmp38 +
|
||||
P(5, 22) * _tmp68 + P(6, 22) * _tmp39 + _tmp72);
|
||||
const Scalar _tmp75 = Scalar(1.0) / (math::max<Scalar>(_tmp74, epsilon));
|
||||
-_tmp36 * _tmp54 -
|
||||
_tmp40 * (_tmp37 * (_tmp14 * (_tmp42 + _tmp47 + _tmp55 + _tmp6) + _tmp32) + _tmp39 * _tmp54) -
|
||||
_tmp54 * cm;
|
||||
const Scalar _tmp57 = _tmp29 * cm;
|
||||
const Scalar _tmp58 = _tmp13 * _tmp37;
|
||||
const Scalar _tmp59 = _tmp25 * _tmp38;
|
||||
const Scalar _tmp60 = _tmp29 * _tmp39;
|
||||
const Scalar _tmp61 = _tmp29 * _tmp36;
|
||||
const Scalar _tmp62 = -_tmp40 * (-_tmp58 - _tmp59 - _tmp60) + _tmp57 + _tmp61;
|
||||
const Scalar _tmp63 = _tmp21 + _tmp55;
|
||||
const Scalar _tmp64 = _tmp40 * (_tmp37 * (_tmp14 * (_tmp2 + _tmp46) + _tmp9 * (_tmp16 + _tmp53) +
|
||||
state(6, 0) * (_tmp52 + _tmp63)) +
|
||||
_tmp38 * (_tmp19 + _tmp9 * (_tmp49 + _tmp63)));
|
||||
const Scalar _tmp65 = -_tmp40 * (_tmp58 + _tmp59 + _tmp60) - _tmp57 - _tmp61;
|
||||
const Scalar _tmp66 = _tmp31 * cm;
|
||||
const Scalar _tmp67 = _tmp31 * _tmp36;
|
||||
const Scalar _tmp68 = _tmp37 * _tmp8;
|
||||
const Scalar _tmp69 = _tmp26 * _tmp38;
|
||||
const Scalar _tmp70 = _tmp31 * _tmp39;
|
||||
const Scalar _tmp71 = -_tmp40 * (_tmp68 + _tmp69 + _tmp70) - _tmp66 - _tmp67;
|
||||
const Scalar _tmp72 = -_tmp40 * (-_tmp68 - _tmp69 - _tmp70) + _tmp66 + _tmp67;
|
||||
const Scalar _tmp73 = P(22, 22) * _tmp62;
|
||||
const Scalar _tmp74 = P(21, 21) * _tmp72;
|
||||
const Scalar _tmp75 =
|
||||
R +
|
||||
_tmp41 * (P(0, 5) * _tmp51 - P(1, 5) * _tmp64 + P(2, 5) * _tmp56 + P(21, 5) * _tmp72 +
|
||||
P(22, 5) * _tmp62 + P(3, 5) * _tmp71 + P(4, 5) * _tmp65 + P(5, 5) * _tmp41) +
|
||||
_tmp51 * (P(0, 0) * _tmp51 - P(1, 0) * _tmp64 + P(2, 0) * _tmp56 + P(21, 0) * _tmp72 +
|
||||
P(22, 0) * _tmp62 + P(3, 0) * _tmp71 + P(4, 0) * _tmp65 + P(5, 0) * _tmp41) +
|
||||
_tmp56 * (P(0, 2) * _tmp51 - P(1, 2) * _tmp64 + P(2, 2) * _tmp56 + P(21, 2) * _tmp72 +
|
||||
P(22, 2) * _tmp62 + P(3, 2) * _tmp71 + P(4, 2) * _tmp65 + P(5, 2) * _tmp41) +
|
||||
_tmp62 * (P(0, 22) * _tmp51 - P(1, 22) * _tmp64 + P(2, 22) * _tmp56 + P(21, 22) * _tmp72 +
|
||||
P(3, 22) * _tmp71 + P(4, 22) * _tmp65 + P(5, 22) * _tmp41 + _tmp73) -
|
||||
_tmp64 * (P(0, 1) * _tmp51 - P(1, 1) * _tmp64 + P(2, 1) * _tmp56 + P(21, 1) * _tmp72 +
|
||||
P(22, 1) * _tmp62 + P(3, 1) * _tmp71 + P(4, 1) * _tmp65 + P(5, 1) * _tmp41) +
|
||||
_tmp65 * (P(0, 4) * _tmp51 - P(1, 4) * _tmp64 + P(2, 4) * _tmp56 + P(21, 4) * _tmp72 +
|
||||
P(22, 4) * _tmp62 + P(3, 4) * _tmp71 + P(4, 4) * _tmp65 + P(5, 4) * _tmp41) +
|
||||
_tmp71 * (P(0, 3) * _tmp51 - P(1, 3) * _tmp64 + P(2, 3) * _tmp56 + P(21, 3) * _tmp72 +
|
||||
P(22, 3) * _tmp62 + P(3, 3) * _tmp71 + P(4, 3) * _tmp65 + P(5, 3) * _tmp41) +
|
||||
_tmp72 * (P(0, 21) * _tmp51 - P(1, 21) * _tmp64 + P(2, 21) * _tmp56 + P(22, 21) * _tmp62 +
|
||||
P(3, 21) * _tmp71 + P(4, 21) * _tmp65 + P(5, 21) * _tmp41 + _tmp74);
|
||||
const Scalar _tmp76 = Scalar(1.0) / (math::max<Scalar>(_tmp75, epsilon));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = _tmp74;
|
||||
_innov_var = _tmp75;
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
|
||||
matrix::Matrix<Scalar, 23, 1>& _k = (*K);
|
||||
|
||||
_k.setZero();
|
||||
|
||||
_k(22, 0) = _tmp75 * (P(22, 0) * _tmp48 + P(22, 1) * _tmp67 + P(22, 2) * _tmp56 +
|
||||
P(22, 23) * _tmp62 + P(22, 3) * _tmp70 + P(22, 4) * _tmp38 +
|
||||
P(22, 5) * _tmp68 + P(22, 6) * _tmp39 + _tmp72);
|
||||
_k(23, 0) = _tmp75 * (P(23, 0) * _tmp48 + P(23, 1) * _tmp67 + P(23, 2) * _tmp56 +
|
||||
P(23, 22) * _tmp71 + P(23, 3) * _tmp70 + P(23, 4) * _tmp38 +
|
||||
P(23, 5) * _tmp68 + P(23, 6) * _tmp39 + _tmp73);
|
||||
_k(21, 0) =
|
||||
_tmp76 * (P(21, 0) * _tmp51 - P(21, 1) * _tmp64 + P(21, 2) * _tmp56 + P(21, 22) * _tmp62 +
|
||||
P(21, 3) * _tmp71 + P(21, 4) * _tmp65 + P(21, 5) * _tmp41 + _tmp74);
|
||||
_k(22, 0) =
|
||||
_tmp76 * (P(22, 0) * _tmp51 - P(22, 1) * _tmp64 + P(22, 2) * _tmp56 + P(22, 21) * _tmp72 +
|
||||
P(22, 3) * _tmp71 + P(22, 4) * _tmp65 + P(22, 5) * _tmp41 + _tmp73);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+71
-74
@@ -17,107 +17,104 @@ namespace sym {
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* P: Matrix23_23
|
||||
* distance: Scalar
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Matrix21
|
||||
* H: Matrix24_1
|
||||
* H: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar distance,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar distance,
|
||||
const Scalar R, const Scalar epsilon,
|
||||
matrix::Matrix<Scalar, 2, 1>* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 291
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
// Total ops: 196
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (28)
|
||||
const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 =
|
||||
// Intermediate terms (33)
|
||||
const Scalar _tmp0 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp2 = 1 - 2 * _tmp1;
|
||||
const Scalar _tmp3 =
|
||||
Scalar(1.0) /
|
||||
(distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
|
||||
const Scalar _tmp2 = _tmp1 * (_tmp0 - 2 * std::pow(state(1, 0), Scalar(2)));
|
||||
const Scalar _tmp3 = 2 * state(1, 0);
|
||||
const Scalar _tmp4 = 2 * state(3, 0);
|
||||
const Scalar _tmp5 = _tmp4 * state(6, 0);
|
||||
const Scalar _tmp6 = _tmp1 * (_tmp3 * state(4, 0) + _tmp5);
|
||||
const Scalar _tmp7 = _tmp3 * state(6, 0);
|
||||
const Scalar _tmp8 = _tmp1 * (-_tmp4 * state(4, 0) + _tmp7);
|
||||
const Scalar _tmp9 = _tmp4 * state(0, 0);
|
||||
const Scalar _tmp10 = _tmp3 * state(2, 0);
|
||||
const Scalar _tmp11 = _tmp1 * (_tmp10 - _tmp9);
|
||||
const Scalar _tmp12 = 2 * state(0, 0);
|
||||
const Scalar _tmp13 = 4 * state(5, 0);
|
||||
const Scalar _tmp14 = 2 * state(2, 0);
|
||||
const Scalar _tmp15 = _tmp14 * state(6, 0);
|
||||
const Scalar _tmp16 = _tmp1 * (-_tmp12 * state(4, 0) - _tmp13 * state(3, 0) + _tmp15);
|
||||
const Scalar _tmp17 = _tmp12 * state(6, 0);
|
||||
const Scalar _tmp18 = _tmp1 * (-_tmp13 * state(1, 0) + _tmp14 * state(4, 0) + _tmp17);
|
||||
const Scalar _tmp19 = _tmp1 * (_tmp12 * state(1, 0) + _tmp4 * state(2, 0));
|
||||
const Scalar _tmp20 = _tmp1 * (_tmp0 - 2 * std::pow(state(2, 0), Scalar(2)));
|
||||
const Scalar _tmp21 = _tmp1 * (_tmp10 + _tmp9);
|
||||
const Scalar _tmp22 = _tmp1 * (-_tmp12 * state(2, 0) + _tmp4 * state(1, 0));
|
||||
const Scalar _tmp23 = 4 * state(4, 0);
|
||||
const Scalar _tmp24 = _tmp1 * (_tmp12 * state(5, 0) - _tmp23 * state(3, 0) + _tmp7);
|
||||
const Scalar _tmp25 = _tmp1 * (-_tmp17 - _tmp23 * state(2, 0) + _tmp3 * state(5, 0));
|
||||
const Scalar _tmp26 = _tmp1 * (-_tmp15 + _tmp4 * state(5, 0));
|
||||
const Scalar _tmp27 = _tmp1 * (_tmp14 * state(5, 0) + _tmp5);
|
||||
const Scalar _tmp4 = _tmp3 * (-2 * _tmp0 + _tmp2);
|
||||
const Scalar _tmp5 = 2 * state(2, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(0, 0);
|
||||
const Scalar _tmp7 = 2 * state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp8 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp9 = 2 * state(0, 0);
|
||||
const Scalar _tmp10 = _tmp9 * state(1, 0);
|
||||
const Scalar _tmp11 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp12 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp13 = -_tmp0;
|
||||
const Scalar _tmp14 = _tmp12 + _tmp13;
|
||||
const Scalar _tmp15 = _tmp3 * (state(4, 0) * (_tmp6 + _tmp7) + state(5, 0) * (-_tmp10 + _tmp8) +
|
||||
state(6, 0) * (_tmp1 - _tmp11 + _tmp14));
|
||||
const Scalar _tmp16 = _tmp9 * state(3, 0);
|
||||
const Scalar _tmp17 = -_tmp16;
|
||||
const Scalar _tmp18 = _tmp5 * state(1, 0);
|
||||
const Scalar _tmp19 = _tmp17 + _tmp18;
|
||||
const Scalar _tmp20 = _tmp19 * _tmp3;
|
||||
const Scalar _tmp21 = _tmp10 + _tmp8;
|
||||
const Scalar _tmp22 = _tmp21 * _tmp3;
|
||||
const Scalar _tmp23 = -_tmp7;
|
||||
const Scalar _tmp24 = -_tmp12;
|
||||
const Scalar _tmp25 = _tmp3 * (state(4, 0) * (_tmp1 + _tmp11 + _tmp13 + _tmp24) +
|
||||
state(5, 0) * (_tmp17 - _tmp18) + state(6, 0) * (_tmp23 + _tmp6));
|
||||
const Scalar _tmp26 = _tmp3 * (-2 * _tmp11 + _tmp2);
|
||||
const Scalar _tmp27 = -_tmp6;
|
||||
const Scalar _tmp28 = -_tmp1 + _tmp11;
|
||||
const Scalar _tmp29 = _tmp3 * (state(4, 0) * (_tmp23 + _tmp27) + state(5, 0) * (_tmp10 - _tmp8) +
|
||||
state(6, 0) * (_tmp0 + _tmp24 + _tmp28));
|
||||
const Scalar _tmp30 =
|
||||
_tmp3 * (_tmp19 * state(4, 0) + _tmp21 * state(6, 0) + state(5, 0) * (_tmp14 + _tmp28));
|
||||
const Scalar _tmp31 = _tmp3 * (_tmp16 + _tmp18);
|
||||
const Scalar _tmp32 = _tmp3 * (_tmp27 + _tmp7);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
matrix::Matrix<Scalar, 2, 1>& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var(0, 0) =
|
||||
R +
|
||||
_tmp11 * (P(0, 4) * _tmp8 + P(1, 4) * _tmp18 + P(2, 4) * _tmp6 + P(3, 4) * _tmp16 +
|
||||
P(4, 4) * _tmp11 + P(5, 4) * _tmp2 + P(6, 4) * _tmp19) +
|
||||
_tmp16 * (P(0, 3) * _tmp8 + P(1, 3) * _tmp18 + P(2, 3) * _tmp6 + P(3, 3) * _tmp16 +
|
||||
P(4, 3) * _tmp11 + P(5, 3) * _tmp2 + P(6, 3) * _tmp19) +
|
||||
_tmp18 * (P(0, 1) * _tmp8 + P(1, 1) * _tmp18 + P(2, 1) * _tmp6 + P(3, 1) * _tmp16 +
|
||||
P(4, 1) * _tmp11 + P(5, 1) * _tmp2 + P(6, 1) * _tmp19) +
|
||||
_tmp19 * (P(0, 6) * _tmp8 + P(1, 6) * _tmp18 + P(2, 6) * _tmp6 + P(3, 6) * _tmp16 +
|
||||
P(4, 6) * _tmp11 + P(5, 6) * _tmp2 + P(6, 6) * _tmp19) +
|
||||
_tmp2 * (P(0, 5) * _tmp8 + P(1, 5) * _tmp18 + P(2, 5) * _tmp6 + P(3, 5) * _tmp16 +
|
||||
P(4, 5) * _tmp11 + P(5, 5) * _tmp2 + P(6, 5) * _tmp19) +
|
||||
_tmp6 * (P(0, 2) * _tmp8 + P(1, 2) * _tmp18 + P(2, 2) * _tmp6 + P(3, 2) * _tmp16 +
|
||||
P(4, 2) * _tmp11 + P(5, 2) * _tmp2 + P(6, 2) * _tmp19) +
|
||||
_tmp8 * (P(0, 0) * _tmp8 + P(1, 0) * _tmp18 + P(2, 0) * _tmp6 + P(3, 0) * _tmp16 +
|
||||
P(4, 0) * _tmp11 + P(5, 0) * _tmp2 + P(6, 0) * _tmp19);
|
||||
_innov_var(1, 0) =
|
||||
R -
|
||||
_tmp20 * (-P(0, 4) * _tmp26 - P(1, 4) * _tmp27 - P(2, 4) * _tmp25 - P(3, 4) * _tmp24 -
|
||||
P(4, 4) * _tmp20 - P(5, 4) * _tmp21 - P(6, 4) * _tmp22) -
|
||||
_tmp21 * (-P(0, 5) * _tmp26 - P(1, 5) * _tmp27 - P(2, 5) * _tmp25 - P(3, 5) * _tmp24 -
|
||||
P(4, 5) * _tmp20 - P(5, 5) * _tmp21 - P(6, 5) * _tmp22) -
|
||||
_tmp22 * (-P(0, 6) * _tmp26 - P(1, 6) * _tmp27 - P(2, 6) * _tmp25 - P(3, 6) * _tmp24 -
|
||||
P(4, 6) * _tmp20 - P(5, 6) * _tmp21 - P(6, 6) * _tmp22) -
|
||||
_tmp24 * (-P(0, 3) * _tmp26 - P(1, 3) * _tmp27 - P(2, 3) * _tmp25 - P(3, 3) * _tmp24 -
|
||||
P(4, 3) * _tmp20 - P(5, 3) * _tmp21 - P(6, 3) * _tmp22) -
|
||||
_tmp25 * (-P(0, 2) * _tmp26 - P(1, 2) * _tmp27 - P(2, 2) * _tmp25 - P(3, 2) * _tmp24 -
|
||||
P(4, 2) * _tmp20 - P(5, 2) * _tmp21 - P(6, 2) * _tmp22) -
|
||||
_tmp26 * (-P(0, 0) * _tmp26 - P(1, 0) * _tmp27 - P(2, 0) * _tmp25 - P(3, 0) * _tmp24 -
|
||||
P(4, 0) * _tmp20 - P(5, 0) * _tmp21 - P(6, 0) * _tmp22) -
|
||||
_tmp27 * (-P(0, 1) * _tmp26 - P(1, 1) * _tmp27 - P(2, 1) * _tmp25 - P(3, 1) * _tmp24 -
|
||||
P(4, 1) * _tmp20 - P(5, 1) * _tmp21 - P(6, 1) * _tmp22);
|
||||
_innov_var(0, 0) = R +
|
||||
_tmp15 * (P(0, 0) * _tmp15 + P(2, 0) * _tmp25 + P(3, 0) * _tmp20 +
|
||||
P(4, 0) * _tmp4 + P(5, 0) * _tmp22) +
|
||||
_tmp20 * (P(0, 3) * _tmp15 + P(2, 3) * _tmp25 + P(3, 3) * _tmp20 +
|
||||
P(4, 3) * _tmp4 + P(5, 3) * _tmp22) +
|
||||
_tmp22 * (P(0, 5) * _tmp15 + P(2, 5) * _tmp25 + P(3, 5) * _tmp20 +
|
||||
P(4, 5) * _tmp4 + P(5, 5) * _tmp22) +
|
||||
_tmp25 * (P(0, 2) * _tmp15 + P(2, 2) * _tmp25 + P(3, 2) * _tmp20 +
|
||||
P(4, 2) * _tmp4 + P(5, 2) * _tmp22) +
|
||||
_tmp4 * (P(0, 4) * _tmp15 + P(2, 4) * _tmp25 + P(3, 4) * _tmp20 +
|
||||
P(4, 4) * _tmp4 + P(5, 4) * _tmp22);
|
||||
_innov_var(1, 0) = R -
|
||||
_tmp26 * (-P(1, 3) * _tmp29 - P(2, 3) * _tmp30 - P(3, 3) * _tmp26 -
|
||||
P(4, 3) * _tmp31 - P(5, 3) * _tmp32) -
|
||||
_tmp29 * (-P(1, 1) * _tmp29 - P(2, 1) * _tmp30 - P(3, 1) * _tmp26 -
|
||||
P(4, 1) * _tmp31 - P(5, 1) * _tmp32) -
|
||||
_tmp30 * (-P(1, 2) * _tmp29 - P(2, 2) * _tmp30 - P(3, 2) * _tmp26 -
|
||||
P(4, 2) * _tmp31 - P(5, 2) * _tmp32) -
|
||||
_tmp31 * (-P(1, 4) * _tmp29 - P(2, 4) * _tmp30 - P(3, 4) * _tmp26 -
|
||||
P(4, 4) * _tmp31 - P(5, 4) * _tmp32) -
|
||||
_tmp32 * (-P(1, 5) * _tmp29 - P(2, 5) * _tmp30 - P(3, 5) * _tmp26 -
|
||||
P(4, 5) * _tmp31 - P(5, 5) * _tmp32);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp8;
|
||||
_h(1, 0) = _tmp18;
|
||||
_h(2, 0) = _tmp6;
|
||||
_h(3, 0) = _tmp16;
|
||||
_h(4, 0) = _tmp11;
|
||||
_h(5, 0) = _tmp2;
|
||||
_h(6, 0) = _tmp19;
|
||||
_h(0, 0) = _tmp15;
|
||||
_h(2, 0) = _tmp25;
|
||||
_h(3, 0) = _tmp20;
|
||||
_h(4, 0) = _tmp4;
|
||||
_h(5, 0) = _tmp22;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+44
-42
@@ -17,76 +17,78 @@ namespace sym {
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* P: Matrix23_23
|
||||
* distance: Scalar
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
* H: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar distance,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar distance,
|
||||
const Scalar R, const Scalar epsilon,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 166
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
// Total ops: 116
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (13)
|
||||
const Scalar _tmp0 =
|
||||
// Intermediate terms (19)
|
||||
const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp2 =
|
||||
Scalar(1.0) /
|
||||
(distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
|
||||
const Scalar _tmp1 =
|
||||
_tmp0 * (-2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1);
|
||||
const Scalar _tmp2 = 2 * state(3, 0);
|
||||
const Scalar _tmp3 = 2 * state(2, 0);
|
||||
const Scalar _tmp4 = _tmp0 * (_tmp2 * state(0, 0) + _tmp3 * state(1, 0));
|
||||
const Scalar _tmp5 = _tmp0 * (_tmp2 * state(1, 0) - _tmp3 * state(0, 0));
|
||||
const Scalar _tmp6 = 4 * state(4, 0);
|
||||
const Scalar _tmp7 = 2 * state(5, 0);
|
||||
const Scalar _tmp8 = 2 * state(6, 0);
|
||||
const Scalar _tmp9 = _tmp0 * (-_tmp6 * state(3, 0) + _tmp7 * state(0, 0) + _tmp8 * state(1, 0));
|
||||
const Scalar _tmp10 = _tmp0 * (-_tmp6 * state(2, 0) + _tmp7 * state(1, 0) - _tmp8 * state(0, 0));
|
||||
const Scalar _tmp11 = _tmp0 * (-_tmp3 * state(6, 0) + _tmp7 * state(3, 0));
|
||||
const Scalar _tmp12 = _tmp0 * (_tmp2 * state(6, 0) + _tmp3 * state(5, 0));
|
||||
const Scalar _tmp3 = _tmp2 * (-2 * _tmp0 - 2 * _tmp1 + 1);
|
||||
const Scalar _tmp4 = -2 * state(0, 0) * state(2, 0);
|
||||
const Scalar _tmp5 = 2 * state(3, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(1, 0);
|
||||
const Scalar _tmp7 = _tmp5 * state(2, 0);
|
||||
const Scalar _tmp8 = 2 * state(1, 0);
|
||||
const Scalar _tmp9 = _tmp8 * state(0, 0);
|
||||
const Scalar _tmp10 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp11 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp12 = -_tmp0 + _tmp1;
|
||||
const Scalar _tmp13 = _tmp2 * (state(4, 0) * (_tmp4 - _tmp6) + state(5, 0) * (-_tmp7 + _tmp9) +
|
||||
state(6, 0) * (-_tmp10 + _tmp11 + _tmp12));
|
||||
const Scalar _tmp14 = _tmp5 * state(0, 0);
|
||||
const Scalar _tmp15 = _tmp8 * state(2, 0);
|
||||
const Scalar _tmp16 =
|
||||
_tmp2 * (state(4, 0) * (-_tmp14 + _tmp15) + state(5, 0) * (_tmp10 - _tmp11 + _tmp12) +
|
||||
state(6, 0) * (_tmp7 + _tmp9));
|
||||
const Scalar _tmp17 = _tmp2 * (_tmp14 + _tmp15);
|
||||
const Scalar _tmp18 = _tmp2 * (_tmp4 + _tmp6);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R -
|
||||
_tmp1 * (-P(0, 4) * _tmp11 - P(1, 4) * _tmp12 - P(2, 4) * _tmp10 -
|
||||
P(3, 4) * _tmp9 - P(4, 4) * _tmp1 - P(5, 4) * _tmp4 - P(6, 4) * _tmp5) -
|
||||
_tmp10 * (-P(0, 2) * _tmp11 - P(1, 2) * _tmp12 - P(2, 2) * _tmp10 -
|
||||
P(3, 2) * _tmp9 - P(4, 2) * _tmp1 - P(5, 2) * _tmp4 - P(6, 2) * _tmp5) -
|
||||
_tmp11 * (-P(0, 0) * _tmp11 - P(1, 0) * _tmp12 - P(2, 0) * _tmp10 -
|
||||
P(3, 0) * _tmp9 - P(4, 0) * _tmp1 - P(5, 0) * _tmp4 - P(6, 0) * _tmp5) -
|
||||
_tmp12 * (-P(0, 1) * _tmp11 - P(1, 1) * _tmp12 - P(2, 1) * _tmp10 -
|
||||
P(3, 1) * _tmp9 - P(4, 1) * _tmp1 - P(5, 1) * _tmp4 - P(6, 1) * _tmp5) -
|
||||
_tmp4 * (-P(0, 5) * _tmp11 - P(1, 5) * _tmp12 - P(2, 5) * _tmp10 -
|
||||
P(3, 5) * _tmp9 - P(4, 5) * _tmp1 - P(5, 5) * _tmp4 - P(6, 5) * _tmp5) -
|
||||
_tmp5 * (-P(0, 6) * _tmp11 - P(1, 6) * _tmp12 - P(2, 6) * _tmp10 -
|
||||
P(3, 6) * _tmp9 - P(4, 6) * _tmp1 - P(5, 6) * _tmp4 - P(6, 6) * _tmp5) -
|
||||
_tmp9 * (-P(0, 3) * _tmp11 - P(1, 3) * _tmp12 - P(2, 3) * _tmp10 -
|
||||
P(3, 3) * _tmp9 - P(4, 3) * _tmp1 - P(5, 3) * _tmp4 - P(6, 3) * _tmp5);
|
||||
_tmp13 * (-P(1, 1) * _tmp13 - P(2, 1) * _tmp16 - P(3, 1) * _tmp3 -
|
||||
P(4, 1) * _tmp17 - P(5, 1) * _tmp18) -
|
||||
_tmp16 * (-P(1, 2) * _tmp13 - P(2, 2) * _tmp16 - P(3, 2) * _tmp3 -
|
||||
P(4, 2) * _tmp17 - P(5, 2) * _tmp18) -
|
||||
_tmp17 * (-P(1, 4) * _tmp13 - P(2, 4) * _tmp16 - P(3, 4) * _tmp3 -
|
||||
P(4, 4) * _tmp17 - P(5, 4) * _tmp18) -
|
||||
_tmp18 * (-P(1, 5) * _tmp13 - P(2, 5) * _tmp16 - P(3, 5) * _tmp3 -
|
||||
P(4, 5) * _tmp17 - P(5, 5) * _tmp18) -
|
||||
_tmp3 * (-P(1, 3) * _tmp13 - P(2, 3) * _tmp16 - P(3, 3) * _tmp3 -
|
||||
P(4, 3) * _tmp17 - P(5, 3) * _tmp18);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = -_tmp11;
|
||||
_h(1, 0) = -_tmp12;
|
||||
_h(2, 0) = -_tmp10;
|
||||
_h(3, 0) = -_tmp9;
|
||||
_h(4, 0) = -_tmp1;
|
||||
_h(5, 0) = -_tmp4;
|
||||
_h(6, 0) = -_tmp5;
|
||||
_h(1, 0) = -_tmp13;
|
||||
_h(2, 0) = -_tmp16;
|
||||
_h(3, 0) = -_tmp3;
|
||||
_h(4, 0) = -_tmp17;
|
||||
_h(5, 0) = -_tmp18;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+46
-44
@@ -17,7 +17,7 @@ namespace sym {
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* P: Matrix23_23
|
||||
* antenna_yaw_offset: Scalar
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
@@ -25,74 +25,76 @@ namespace sym {
|
||||
* Outputs:
|
||||
* meas_pred: Scalar
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
* H: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P,
|
||||
const Scalar antenna_yaw_offset, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const meas_pred = nullptr,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 105
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
// Total ops: 95
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (22)
|
||||
const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::sin(antenna_yaw_offset);
|
||||
const Scalar _tmp2 = 2 * state(0, 0) * state(3, 0);
|
||||
const Scalar _tmp3 = 2 * state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp4 = std::cos(antenna_yaw_offset);
|
||||
const Scalar _tmp5 =
|
||||
_tmp1 * (_tmp0 - 2 * std::pow(state(1, 0), Scalar(2))) + _tmp4 * (_tmp2 + _tmp3);
|
||||
const Scalar _tmp6 =
|
||||
_tmp1 * (-_tmp2 + _tmp3) + _tmp4 * (_tmp0 - 2 * std::pow(state(2, 0), Scalar(2)));
|
||||
const Scalar _tmp7 = _tmp6 + epsilon * ((((_tmp6) > 0) - ((_tmp6) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp8 = 4 * _tmp1;
|
||||
const Scalar _tmp9 = 2 * _tmp4;
|
||||
const Scalar _tmp10 = Scalar(1.0) / (_tmp7);
|
||||
const Scalar _tmp11 = 4 * _tmp4;
|
||||
const Scalar _tmp12 = 2 * _tmp1;
|
||||
const Scalar _tmp13 = std::pow(_tmp7, Scalar(2));
|
||||
const Scalar _tmp14 = _tmp5 / _tmp13;
|
||||
const Scalar _tmp15 = _tmp13 / (_tmp13 + std::pow(_tmp5, Scalar(2)));
|
||||
const Scalar _tmp16 = _tmp15 * (_tmp10 * (-_tmp8 * state(3, 0) + _tmp9 * state(0, 0)) -
|
||||
_tmp14 * (-_tmp11 * state(3, 0) - _tmp12 * state(0, 0)));
|
||||
const Scalar _tmp17 = _tmp12 * _tmp14;
|
||||
const Scalar _tmp18 = _tmp10 * _tmp9;
|
||||
const Scalar _tmp19 = _tmp15 * (_tmp17 * state(3, 0) + _tmp18 * state(3, 0));
|
||||
const Scalar _tmp20 =
|
||||
_tmp15 * (-_tmp14 * (-_tmp11 * state(2, 0) + _tmp12 * state(1, 0)) + _tmp18 * state(1, 0));
|
||||
const Scalar _tmp21 =
|
||||
_tmp15 * (_tmp10 * (-_tmp8 * state(1, 0) + _tmp9 * state(2, 0)) - _tmp17 * state(2, 0));
|
||||
// Intermediate terms (28)
|
||||
const Scalar _tmp0 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp2 = 1 - 2 * _tmp1;
|
||||
const Scalar _tmp3 = std::sin(antenna_yaw_offset);
|
||||
const Scalar _tmp4 = 2 * state(0, 0);
|
||||
const Scalar _tmp5 = _tmp4 * state(3, 0);
|
||||
const Scalar _tmp6 = 2 * state(2, 0);
|
||||
const Scalar _tmp7 = _tmp6 * state(1, 0);
|
||||
const Scalar _tmp8 = std::cos(antenna_yaw_offset);
|
||||
const Scalar _tmp9 = _tmp3 * (-2 * _tmp0 + _tmp2) + _tmp8 * (_tmp5 + _tmp7);
|
||||
const Scalar _tmp10 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp11 = -_tmp5;
|
||||
const Scalar _tmp12 = _tmp11 + _tmp7;
|
||||
const Scalar _tmp13 = _tmp12 * _tmp3 + _tmp8 * (-2 * _tmp10 + _tmp2);
|
||||
const Scalar _tmp14 = _tmp13 + epsilon * ((((_tmp13) > 0) - ((_tmp13) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp15 = _tmp6 * state(0, 0);
|
||||
const Scalar _tmp16 = 2 * state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp17 = std::pow(_tmp14, Scalar(2));
|
||||
const Scalar _tmp18 = _tmp9 / _tmp17;
|
||||
const Scalar _tmp19 = _tmp6 * state(3, 0);
|
||||
const Scalar _tmp20 = _tmp4 * state(1, 0);
|
||||
const Scalar _tmp21 = Scalar(1.0) / (_tmp14);
|
||||
const Scalar _tmp22 = _tmp17 / (_tmp17 + std::pow(_tmp9, Scalar(2)));
|
||||
const Scalar _tmp23 =
|
||||
_tmp22 * (-_tmp18 * _tmp8 * (-_tmp15 - _tmp16) + _tmp21 * _tmp8 * (-_tmp19 + _tmp20));
|
||||
const Scalar _tmp24 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp25 = -_tmp0 + _tmp10;
|
||||
const Scalar _tmp26 =
|
||||
_tmp22 * (-_tmp18 * (_tmp12 * _tmp8 + _tmp3 * (_tmp1 - _tmp24 + _tmp25)) +
|
||||
_tmp21 * (_tmp3 * (_tmp11 - _tmp7) + _tmp8 * (-_tmp1 + _tmp24 + _tmp25)));
|
||||
const Scalar _tmp27 =
|
||||
_tmp22 * (-_tmp18 * _tmp3 * (_tmp15 + _tmp16) + _tmp21 * _tmp3 * (_tmp19 - _tmp20));
|
||||
|
||||
// Output terms (3)
|
||||
if (meas_pred != nullptr) {
|
||||
Scalar& _meas_pred = (*meas_pred);
|
||||
|
||||
_meas_pred = std::atan2(_tmp5, _tmp7);
|
||||
_meas_pred = std::atan2(_tmp9, _tmp14);
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var =
|
||||
R + _tmp16 * (P(0, 3) * _tmp19 + P(1, 3) * _tmp21 + P(2, 3) * _tmp20 + P(3, 3) * _tmp16) +
|
||||
_tmp19 * (P(0, 0) * _tmp19 + P(1, 0) * _tmp21 + P(2, 0) * _tmp20 + P(3, 0) * _tmp16) +
|
||||
_tmp20 * (P(0, 2) * _tmp19 + P(1, 2) * _tmp21 + P(2, 2) * _tmp20 + P(3, 2) * _tmp16) +
|
||||
_tmp21 * (P(0, 1) * _tmp19 + P(1, 1) * _tmp21 + P(2, 1) * _tmp20 + P(3, 1) * _tmp16);
|
||||
_innov_var = R + _tmp23 * (P(0, 1) * _tmp27 + P(1, 1) * _tmp23 + P(2, 1) * _tmp26) +
|
||||
_tmp26 * (P(0, 2) * _tmp27 + P(1, 2) * _tmp23 + P(2, 2) * _tmp26) +
|
||||
_tmp27 * (P(0, 0) * _tmp27 + P(1, 0) * _tmp23 + P(2, 0) * _tmp26);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp19;
|
||||
_h(1, 0) = _tmp21;
|
||||
_h(2, 0) = _tmp20;
|
||||
_h(3, 0) = _tmp16;
|
||||
_h(0, 0) = _tmp27;
|
||||
_h(1, 0) = _tmp23;
|
||||
_h(2, 0) = _tmp26;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+124
-165
@@ -17,7 +17,7 @@ namespace sym {
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* P: Matrix23_23
|
||||
* meas: Matrix31
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
@@ -25,206 +25,165 @@ namespace sym {
|
||||
* Outputs:
|
||||
* innov: Matrix31
|
||||
* innov_var: Matrix31
|
||||
* Kx: Matrix24_1
|
||||
* Ky: Matrix24_1
|
||||
* Kz: Matrix24_1
|
||||
* Kx: Matrix23_1
|
||||
* Ky: Matrix23_1
|
||||
* Kz: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P,
|
||||
const matrix::Matrix<Scalar, 3, 1>& meas, const Scalar R,
|
||||
const Scalar epsilon,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov = nullptr,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const Kx = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const Ky = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const Kz = nullptr) {
|
||||
// Total ops: 617
|
||||
matrix::Matrix<Scalar, 23, 1>* const Kx = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Ky = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Kz = nullptr) {
|
||||
// Total ops: 361
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (34)
|
||||
// Intermediate terms (31)
|
||||
const Scalar _tmp0 = 2 * state(2, 0);
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 =
|
||||
const Scalar _tmp1 = _tmp0 * state(0, 0);
|
||||
const Scalar _tmp2 = 2 * state(1, 0);
|
||||
const Scalar _tmp3 = _tmp2 * state(3, 0);
|
||||
const Scalar _tmp4 = -_tmp1 + _tmp3;
|
||||
const Scalar _tmp5 =
|
||||
Scalar(9.8066499999999994) /
|
||||
std::sqrt(Scalar(epsilon + std::pow(meas(0, 0), Scalar(2)) + std::pow(meas(1, 0), Scalar(2)) +
|
||||
std::pow(meas(2, 0), Scalar(2))));
|
||||
const Scalar _tmp3 = Scalar(19.613299999999999) * state(1, 0);
|
||||
const Scalar _tmp4 = -P(3, 0) * _tmp3;
|
||||
const Scalar _tmp5 = Scalar(19.613299999999999) * state(2, 0);
|
||||
const Scalar _tmp6 = P(0, 0) * _tmp5;
|
||||
const Scalar _tmp7 = Scalar(19.613299999999999) * state(0, 0);
|
||||
const Scalar _tmp8 = Scalar(19.613299999999999) * state(3, 0);
|
||||
const Scalar _tmp9 = P(2, 1) * _tmp7;
|
||||
const Scalar _tmp10 = -P(1, 1) * _tmp8;
|
||||
const Scalar _tmp11 = P(2, 2) * _tmp7;
|
||||
const Scalar _tmp12 = -P(1, 2) * _tmp8;
|
||||
const Scalar _tmp13 = -P(3, 3) * _tmp3;
|
||||
const Scalar _tmp14 = P(0, 3) * _tmp5;
|
||||
const Scalar _tmp15 = R - _tmp3 * (-P(1, 3) * _tmp8 + P(2, 3) * _tmp7 + _tmp13 + _tmp14) +
|
||||
_tmp5 * (-P(1, 0) * _tmp8 + P(2, 0) * _tmp7 + _tmp4 + _tmp6) +
|
||||
_tmp7 * (P(0, 2) * _tmp5 - P(3, 2) * _tmp3 + _tmp11 + _tmp12) -
|
||||
_tmp8 * (P(0, 1) * _tmp5 - P(3, 1) * _tmp3 + _tmp10 + _tmp9);
|
||||
const Scalar _tmp16 = P(3, 0) * _tmp5;
|
||||
const Scalar _tmp17 = -P(0, 0) * _tmp3;
|
||||
const Scalar _tmp18 = -P(2, 2) * _tmp8;
|
||||
const Scalar _tmp19 = P(1, 2) * _tmp7;
|
||||
const Scalar _tmp20 = -P(2, 1) * _tmp8;
|
||||
const Scalar _tmp21 = -P(1, 1) * _tmp7;
|
||||
const Scalar _tmp22 = -P(3, 3) * _tmp5;
|
||||
const Scalar _tmp23 = -P(0, 3) * _tmp3;
|
||||
const Scalar _tmp24 = R - _tmp3 * (-P(1, 0) * _tmp7 - P(2, 0) * _tmp8 - _tmp16 + _tmp17) -
|
||||
_tmp5 * (-P(1, 3) * _tmp7 - P(2, 3) * _tmp8 + _tmp22 + _tmp23) -
|
||||
_tmp7 * (-P(0, 1) * _tmp3 - P(3, 1) * _tmp5 + _tmp20 + _tmp21) -
|
||||
_tmp8 * (-P(0, 2) * _tmp3 - P(3, 2) * _tmp5 + _tmp18 - _tmp19);
|
||||
const Scalar _tmp25 = Scalar(39.226599999999998) * state(2, 0);
|
||||
const Scalar _tmp26 = P(2, 2) * _tmp25;
|
||||
const Scalar _tmp27 = Scalar(39.226599999999998) * state(1, 0);
|
||||
const Scalar _tmp28 = P(1, 1) * _tmp27;
|
||||
const Scalar _tmp29 =
|
||||
R + _tmp25 * (P(1, 2) * _tmp27 + _tmp26) + _tmp27 * (P(2, 1) * _tmp25 + _tmp28);
|
||||
const Scalar _tmp30 = Scalar(1.0) / (_tmp15);
|
||||
const Scalar _tmp31 = Scalar(19.613299999999999) * P(6, 3);
|
||||
const Scalar _tmp32 = Scalar(1.0) / (_tmp24);
|
||||
const Scalar _tmp33 = Scalar(1.0) / (_tmp29);
|
||||
const Scalar _tmp6 = _tmp0 * state(3, 0);
|
||||
const Scalar _tmp7 = _tmp2 * state(0, 0);
|
||||
const Scalar _tmp8 = _tmp6 + _tmp7;
|
||||
const Scalar _tmp9 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp10 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp11 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp12 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp13 = -Scalar(9.8066499999999994) * _tmp10 + Scalar(9.8066499999999994) * _tmp11 +
|
||||
Scalar(9.8066499999999994) * _tmp12 - Scalar(9.8066499999999994) * _tmp9;
|
||||
const Scalar _tmp14 = -Scalar(9.8066499999999994) * _tmp8;
|
||||
const Scalar _tmp15 = P(2, 2) * _tmp14;
|
||||
const Scalar _tmp16 = P(1, 1) * _tmp13;
|
||||
const Scalar _tmp17 =
|
||||
R + _tmp13 * (P(2, 1) * _tmp14 + _tmp16) + _tmp14 * (P(1, 2) * _tmp13 + _tmp15);
|
||||
const Scalar _tmp18 = Scalar(9.8066499999999994) * _tmp10 - Scalar(9.8066499999999994) * _tmp11 -
|
||||
Scalar(9.8066499999999994) * _tmp12 + Scalar(9.8066499999999994) * _tmp9;
|
||||
const Scalar _tmp19 = -Scalar(9.8066499999999994) * _tmp1 + Scalar(9.8066499999999994) * _tmp3;
|
||||
const Scalar _tmp20 = P(2, 2) * _tmp19;
|
||||
const Scalar _tmp21 = P(0, 0) * _tmp18;
|
||||
const Scalar _tmp22 =
|
||||
R + _tmp18 * (P(2, 0) * _tmp19 + _tmp21) + _tmp19 * (P(0, 2) * _tmp18 + _tmp20);
|
||||
const Scalar _tmp23 = Scalar(9.8066499999999994) * _tmp6 + Scalar(9.8066499999999994) * _tmp7;
|
||||
const Scalar _tmp24 = -Scalar(9.8066499999999994) * _tmp4;
|
||||
const Scalar _tmp25 = P(1, 1) * _tmp24;
|
||||
const Scalar _tmp26 = P(0, 0) * _tmp23;
|
||||
const Scalar _tmp27 =
|
||||
R + _tmp23 * (P(1, 0) * _tmp24 + _tmp26) + _tmp24 * (P(0, 1) * _tmp23 + _tmp25);
|
||||
const Scalar _tmp28 = Scalar(1.0) / (_tmp17);
|
||||
const Scalar _tmp29 = Scalar(1.0) / (_tmp22);
|
||||
const Scalar _tmp30 = Scalar(1.0) / (_tmp27);
|
||||
|
||||
// Output terms (5)
|
||||
if (innov != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _innov = (*innov);
|
||||
|
||||
_innov(0, 0) = Scalar(9.8066499999999994) * _tmp0 * state(0, 0) -
|
||||
Scalar(9.8066499999999994) * _tmp1 * state(3, 0) - _tmp2 * meas(0, 0);
|
||||
_innov(1, 0) = -Scalar(9.8066499999999994) * _tmp0 * state(3, 0) -
|
||||
Scalar(9.8066499999999994) * _tmp1 * state(0, 0) - _tmp2 * meas(1, 0);
|
||||
_innov(2, 0) =
|
||||
-_tmp2 * meas(2, 0) + Scalar(19.613299999999999) * std::pow(state(1, 0), Scalar(2)) +
|
||||
Scalar(19.613299999999999) * std::pow(state(2, 0), Scalar(2)) + Scalar(-9.8066499999999994);
|
||||
_innov(0, 0) = -Scalar(9.8066499999999994) * _tmp4 - _tmp5 * meas(0, 0);
|
||||
_innov(1, 0) = -_tmp5 * meas(1, 0) - Scalar(9.8066499999999994) * _tmp8;
|
||||
_innov(2, 0) = Scalar(19.613299999999999) * _tmp10 - _tmp5 * meas(2, 0) +
|
||||
Scalar(19.613299999999999) * _tmp9 + Scalar(-9.8066499999999994);
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var(0, 0) = _tmp15;
|
||||
_innov_var(1, 0) = _tmp24;
|
||||
_innov_var(2, 0) = _tmp29;
|
||||
_innov_var(0, 0) = _tmp17;
|
||||
_innov_var(1, 0) = _tmp22;
|
||||
_innov_var(2, 0) = _tmp27;
|
||||
}
|
||||
|
||||
if (Kx != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _kx = (*Kx);
|
||||
matrix::Matrix<Scalar, 23, 1>& _kx = (*Kx);
|
||||
|
||||
_kx(0, 0) = _tmp30 * (-P(0, 1) * _tmp8 + P(0, 2) * _tmp7 + _tmp23 + _tmp6);
|
||||
_kx(1, 0) = _tmp30 * (P(1, 0) * _tmp5 - P(1, 3) * _tmp3 + _tmp10 + _tmp19);
|
||||
_kx(2, 0) = _tmp30 * (P(2, 0) * _tmp5 - P(2, 3) * _tmp3 + _tmp11 + _tmp20);
|
||||
_kx(3, 0) = _tmp30 * (-P(3, 1) * _tmp8 + P(3, 2) * _tmp7 + _tmp13 + _tmp16);
|
||||
_kx(4, 0) = _tmp30 * (P(4, 0) * _tmp5 - P(4, 1) * _tmp8 + P(4, 2) * _tmp7 - P(4, 3) * _tmp3);
|
||||
_kx(5, 0) = _tmp30 * (P(5, 0) * _tmp5 - P(5, 1) * _tmp8 + P(5, 2) * _tmp7 - P(5, 3) * _tmp3);
|
||||
_kx(6, 0) =
|
||||
_tmp30 * (P(6, 0) * _tmp5 - P(6, 1) * _tmp8 + P(6, 2) * _tmp7 - _tmp31 * state(1, 0));
|
||||
_kx(7, 0) = _tmp30 * (P(7, 0) * _tmp5 - P(7, 1) * _tmp8 + P(7, 2) * _tmp7 - P(7, 3) * _tmp3);
|
||||
_kx(8, 0) = _tmp30 * (P(8, 0) * _tmp5 - P(8, 1) * _tmp8 + P(8, 2) * _tmp7 - P(8, 3) * _tmp3);
|
||||
_kx(9, 0) = _tmp30 * (P(9, 0) * _tmp5 - P(9, 1) * _tmp8 + P(9, 2) * _tmp7 - P(9, 3) * _tmp3);
|
||||
_kx(10, 0) =
|
||||
_tmp30 * (P(10, 0) * _tmp5 - P(10, 1) * _tmp8 + P(10, 2) * _tmp7 - P(10, 3) * _tmp3);
|
||||
_kx(11, 0) =
|
||||
_tmp30 * (P(11, 0) * _tmp5 - P(11, 1) * _tmp8 + P(11, 2) * _tmp7 - P(11, 3) * _tmp3);
|
||||
_kx(12, 0) =
|
||||
_tmp30 * (P(12, 0) * _tmp5 - P(12, 1) * _tmp8 + P(12, 2) * _tmp7 - P(12, 3) * _tmp3);
|
||||
_kx(13, 0) =
|
||||
_tmp30 * (P(13, 0) * _tmp5 - P(13, 1) * _tmp8 + P(13, 2) * _tmp7 - P(13, 3) * _tmp3);
|
||||
_kx(14, 0) =
|
||||
_tmp30 * (P(14, 0) * _tmp5 - P(14, 1) * _tmp8 + P(14, 2) * _tmp7 - P(14, 3) * _tmp3);
|
||||
_kx(15, 0) =
|
||||
_tmp30 * (P(15, 0) * _tmp5 - P(15, 1) * _tmp8 + P(15, 2) * _tmp7 - P(15, 3) * _tmp3);
|
||||
_kx(16, 0) =
|
||||
_tmp30 * (P(16, 0) * _tmp5 - P(16, 1) * _tmp8 + P(16, 2) * _tmp7 - P(16, 3) * _tmp3);
|
||||
_kx(17, 0) =
|
||||
_tmp30 * (P(17, 0) * _tmp5 - P(17, 1) * _tmp8 + P(17, 2) * _tmp7 - P(17, 3) * _tmp3);
|
||||
_kx(18, 0) =
|
||||
_tmp30 * (P(18, 0) * _tmp5 - P(18, 1) * _tmp8 + P(18, 2) * _tmp7 - P(18, 3) * _tmp3);
|
||||
_kx(19, 0) =
|
||||
_tmp30 * (P(19, 0) * _tmp5 - P(19, 1) * _tmp8 + P(19, 2) * _tmp7 - P(19, 3) * _tmp3);
|
||||
_kx(20, 0) =
|
||||
_tmp30 * (P(20, 0) * _tmp5 - P(20, 1) * _tmp8 + P(20, 2) * _tmp7 - P(20, 3) * _tmp3);
|
||||
_kx(21, 0) =
|
||||
_tmp30 * (P(21, 0) * _tmp5 - P(21, 1) * _tmp8 + P(21, 2) * _tmp7 - P(21, 3) * _tmp3);
|
||||
_kx(22, 0) =
|
||||
_tmp30 * (P(22, 0) * _tmp5 - P(22, 1) * _tmp8 + P(22, 2) * _tmp7 - P(22, 3) * _tmp3);
|
||||
_kx(23, 0) =
|
||||
_tmp30 * (P(23, 0) * _tmp5 - P(23, 1) * _tmp8 + P(23, 2) * _tmp7 - P(23, 3) * _tmp3);
|
||||
_kx(0, 0) = _tmp28 * (P(0, 1) * _tmp13 + P(0, 2) * _tmp14);
|
||||
_kx(1, 0) = _tmp28 * (P(1, 2) * _tmp14 + _tmp16);
|
||||
_kx(2, 0) = _tmp28 * (P(2, 1) * _tmp13 + _tmp15);
|
||||
_kx(3, 0) = _tmp28 * (P(3, 1) * _tmp13 + P(3, 2) * _tmp14);
|
||||
_kx(4, 0) = _tmp28 * (P(4, 1) * _tmp13 + P(4, 2) * _tmp14);
|
||||
_kx(5, 0) = _tmp28 * (P(5, 1) * _tmp13 + P(5, 2) * _tmp14);
|
||||
_kx(6, 0) = _tmp28 * (P(6, 1) * _tmp13 + P(6, 2) * _tmp14);
|
||||
_kx(7, 0) = _tmp28 * (P(7, 1) * _tmp13 + P(7, 2) * _tmp14);
|
||||
_kx(8, 0) = _tmp28 * (P(8, 1) * _tmp13 + P(8, 2) * _tmp14);
|
||||
_kx(9, 0) = _tmp28 * (P(9, 1) * _tmp13 + P(9, 2) * _tmp14);
|
||||
_kx(10, 0) = _tmp28 * (P(10, 1) * _tmp13 + P(10, 2) * _tmp14);
|
||||
_kx(11, 0) = _tmp28 * (P(11, 1) * _tmp13 + P(11, 2) * _tmp14);
|
||||
_kx(12, 0) = _tmp28 * (P(12, 1) * _tmp13 + P(12, 2) * _tmp14);
|
||||
_kx(13, 0) = _tmp28 * (P(13, 1) * _tmp13 + P(13, 2) * _tmp14);
|
||||
_kx(14, 0) = _tmp28 * (P(14, 1) * _tmp13 + P(14, 2) * _tmp14);
|
||||
_kx(15, 0) = _tmp28 * (P(15, 1) * _tmp13 + P(15, 2) * _tmp14);
|
||||
_kx(16, 0) = _tmp28 * (P(16, 1) * _tmp13 + P(16, 2) * _tmp14);
|
||||
_kx(17, 0) = _tmp28 * (P(17, 1) * _tmp13 + P(17, 2) * _tmp14);
|
||||
_kx(18, 0) = _tmp28 * (P(18, 1) * _tmp13 + P(18, 2) * _tmp14);
|
||||
_kx(19, 0) = _tmp28 * (P(19, 1) * _tmp13 + P(19, 2) * _tmp14);
|
||||
_kx(20, 0) = _tmp28 * (P(20, 1) * _tmp13 + P(20, 2) * _tmp14);
|
||||
_kx(21, 0) = _tmp28 * (P(21, 1) * _tmp13 + P(21, 2) * _tmp14);
|
||||
_kx(22, 0) = _tmp28 * (P(22, 1) * _tmp13 + P(22, 2) * _tmp14);
|
||||
}
|
||||
|
||||
if (Ky != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _ky = (*Ky);
|
||||
matrix::Matrix<Scalar, 23, 1>& _ky = (*Ky);
|
||||
|
||||
_ky(0, 0) = _tmp32 * (-P(0, 1) * _tmp7 - P(0, 2) * _tmp8 - _tmp14 + _tmp17);
|
||||
_ky(1, 0) = _tmp32 * (-P(1, 0) * _tmp3 - P(1, 3) * _tmp5 + _tmp12 + _tmp21);
|
||||
_ky(2, 0) = _tmp32 * (-P(2, 0) * _tmp3 - P(2, 3) * _tmp5 + _tmp18 - _tmp9);
|
||||
_ky(3, 0) = _tmp32 * (-P(3, 1) * _tmp7 - P(3, 2) * _tmp8 + _tmp22 + _tmp4);
|
||||
_ky(4, 0) = _tmp32 * (-P(4, 0) * _tmp3 - P(4, 1) * _tmp7 - P(4, 2) * _tmp8 - P(4, 3) * _tmp5);
|
||||
_ky(5, 0) = _tmp32 * (-P(5, 0) * _tmp3 - P(5, 1) * _tmp7 - P(5, 2) * _tmp8 - P(5, 3) * _tmp5);
|
||||
_ky(6, 0) =
|
||||
_tmp32 * (-P(6, 0) * _tmp3 - P(6, 1) * _tmp7 - P(6, 2) * _tmp8 - _tmp31 * state(2, 0));
|
||||
_ky(7, 0) = _tmp32 * (-P(7, 0) * _tmp3 - P(7, 1) * _tmp7 - P(7, 2) * _tmp8 - P(7, 3) * _tmp5);
|
||||
_ky(8, 0) = _tmp32 * (-P(8, 0) * _tmp3 - P(8, 1) * _tmp7 - P(8, 2) * _tmp8 - P(8, 3) * _tmp5);
|
||||
_ky(9, 0) = _tmp32 * (-P(9, 0) * _tmp3 - P(9, 1) * _tmp7 - P(9, 2) * _tmp8 - P(9, 3) * _tmp5);
|
||||
_ky(10, 0) =
|
||||
_tmp32 * (-P(10, 0) * _tmp3 - P(10, 1) * _tmp7 - P(10, 2) * _tmp8 - P(10, 3) * _tmp5);
|
||||
_ky(11, 0) =
|
||||
_tmp32 * (-P(11, 0) * _tmp3 - P(11, 1) * _tmp7 - P(11, 2) * _tmp8 - P(11, 3) * _tmp5);
|
||||
_ky(12, 0) =
|
||||
_tmp32 * (-P(12, 0) * _tmp3 - P(12, 1) * _tmp7 - P(12, 2) * _tmp8 - P(12, 3) * _tmp5);
|
||||
_ky(13, 0) =
|
||||
_tmp32 * (-P(13, 0) * _tmp3 - P(13, 1) * _tmp7 - P(13, 2) * _tmp8 - P(13, 3) * _tmp5);
|
||||
_ky(14, 0) =
|
||||
_tmp32 * (-P(14, 0) * _tmp3 - P(14, 1) * _tmp7 - P(14, 2) * _tmp8 - P(14, 3) * _tmp5);
|
||||
_ky(15, 0) =
|
||||
_tmp32 * (-P(15, 0) * _tmp3 - P(15, 1) * _tmp7 - P(15, 2) * _tmp8 - P(15, 3) * _tmp5);
|
||||
_ky(16, 0) =
|
||||
_tmp32 * (-P(16, 0) * _tmp3 - P(16, 1) * _tmp7 - P(16, 2) * _tmp8 - P(16, 3) * _tmp5);
|
||||
_ky(17, 0) =
|
||||
_tmp32 * (-P(17, 0) * _tmp3 - P(17, 1) * _tmp7 - P(17, 2) * _tmp8 - P(17, 3) * _tmp5);
|
||||
_ky(18, 0) =
|
||||
_tmp32 * (-P(18, 0) * _tmp3 - P(18, 1) * _tmp7 - P(18, 2) * _tmp8 - P(18, 3) * _tmp5);
|
||||
_ky(19, 0) =
|
||||
_tmp32 * (-P(19, 0) * _tmp3 - P(19, 1) * _tmp7 - P(19, 2) * _tmp8 - P(19, 3) * _tmp5);
|
||||
_ky(20, 0) =
|
||||
_tmp32 * (-P(20, 0) * _tmp3 - P(20, 1) * _tmp7 - P(20, 2) * _tmp8 - P(20, 3) * _tmp5);
|
||||
_ky(21, 0) =
|
||||
_tmp32 * (-P(21, 0) * _tmp3 - P(21, 1) * _tmp7 - P(21, 2) * _tmp8 - P(21, 3) * _tmp5);
|
||||
_ky(22, 0) =
|
||||
_tmp32 * (-P(22, 0) * _tmp3 - P(22, 1) * _tmp7 - P(22, 2) * _tmp8 - P(22, 3) * _tmp5);
|
||||
_ky(23, 0) =
|
||||
_tmp32 * (-P(23, 0) * _tmp3 - P(23, 1) * _tmp7 - P(23, 2) * _tmp8 - P(23, 3) * _tmp5);
|
||||
_ky(0, 0) = _tmp29 * (P(0, 2) * _tmp19 + _tmp21);
|
||||
_ky(1, 0) = _tmp29 * (P(1, 0) * _tmp18 + P(1, 2) * _tmp19);
|
||||
_ky(2, 0) = _tmp29 * (P(2, 0) * _tmp18 + _tmp20);
|
||||
_ky(3, 0) = _tmp29 * (P(3, 0) * _tmp18 + P(3, 2) * _tmp19);
|
||||
_ky(4, 0) = _tmp29 * (P(4, 0) * _tmp18 + P(4, 2) * _tmp19);
|
||||
_ky(5, 0) = _tmp29 * (P(5, 0) * _tmp18 + P(5, 2) * _tmp19);
|
||||
_ky(6, 0) = _tmp29 * (P(6, 0) * _tmp18 + P(6, 2) * _tmp19);
|
||||
_ky(7, 0) = _tmp29 * (P(7, 0) * _tmp18 + P(7, 2) * _tmp19);
|
||||
_ky(8, 0) = _tmp29 * (P(8, 0) * _tmp18 + P(8, 2) * _tmp19);
|
||||
_ky(9, 0) = _tmp29 * (P(9, 0) * _tmp18 + P(9, 2) * _tmp19);
|
||||
_ky(10, 0) = _tmp29 * (P(10, 0) * _tmp18 + P(10, 2) * _tmp19);
|
||||
_ky(11, 0) = _tmp29 * (P(11, 0) * _tmp18 + P(11, 2) * _tmp19);
|
||||
_ky(12, 0) = _tmp29 * (P(12, 0) * _tmp18 + P(12, 2) * _tmp19);
|
||||
_ky(13, 0) = _tmp29 * (P(13, 0) * _tmp18 + P(13, 2) * _tmp19);
|
||||
_ky(14, 0) = _tmp29 * (P(14, 0) * _tmp18 + P(14, 2) * _tmp19);
|
||||
_ky(15, 0) = _tmp29 * (P(15, 0) * _tmp18 + P(15, 2) * _tmp19);
|
||||
_ky(16, 0) = _tmp29 * (P(16, 0) * _tmp18 + P(16, 2) * _tmp19);
|
||||
_ky(17, 0) = _tmp29 * (P(17, 0) * _tmp18 + P(17, 2) * _tmp19);
|
||||
_ky(18, 0) = _tmp29 * (P(18, 0) * _tmp18 + P(18, 2) * _tmp19);
|
||||
_ky(19, 0) = _tmp29 * (P(19, 0) * _tmp18 + P(19, 2) * _tmp19);
|
||||
_ky(20, 0) = _tmp29 * (P(20, 0) * _tmp18 + P(20, 2) * _tmp19);
|
||||
_ky(21, 0) = _tmp29 * (P(21, 0) * _tmp18 + P(21, 2) * _tmp19);
|
||||
_ky(22, 0) = _tmp29 * (P(22, 0) * _tmp18 + P(22, 2) * _tmp19);
|
||||
}
|
||||
|
||||
if (Kz != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _kz = (*Kz);
|
||||
matrix::Matrix<Scalar, 23, 1>& _kz = (*Kz);
|
||||
|
||||
_kz(0, 0) = _tmp33 * (P(0, 1) * _tmp27 + P(0, 2) * _tmp25);
|
||||
_kz(1, 0) = _tmp33 * (P(1, 2) * _tmp25 + _tmp28);
|
||||
_kz(2, 0) = _tmp33 * (P(2, 1) * _tmp27 + _tmp26);
|
||||
_kz(3, 0) = _tmp33 * (P(3, 1) * _tmp27 + P(3, 2) * _tmp25);
|
||||
_kz(4, 0) = _tmp33 * (P(4, 1) * _tmp27 + P(4, 2) * _tmp25);
|
||||
_kz(5, 0) = _tmp33 * (P(5, 1) * _tmp27 + P(5, 2) * _tmp25);
|
||||
_kz(6, 0) = _tmp33 * (P(6, 1) * _tmp27 + P(6, 2) * _tmp25);
|
||||
_kz(7, 0) = _tmp33 * (P(7, 1) * _tmp27 + P(7, 2) * _tmp25);
|
||||
_kz(8, 0) = _tmp33 * (P(8, 1) * _tmp27 + P(8, 2) * _tmp25);
|
||||
_kz(9, 0) = _tmp33 * (P(9, 1) * _tmp27 + P(9, 2) * _tmp25);
|
||||
_kz(10, 0) = _tmp33 * (P(10, 1) * _tmp27 + P(10, 2) * _tmp25);
|
||||
_kz(11, 0) = _tmp33 * (P(11, 1) * _tmp27 + P(11, 2) * _tmp25);
|
||||
_kz(12, 0) = _tmp33 * (P(12, 1) * _tmp27 + P(12, 2) * _tmp25);
|
||||
_kz(13, 0) = _tmp33 * (P(13, 1) * _tmp27 + P(13, 2) * _tmp25);
|
||||
_kz(14, 0) = _tmp33 * (P(14, 1) * _tmp27 + P(14, 2) * _tmp25);
|
||||
_kz(15, 0) = _tmp33 * (P(15, 1) * _tmp27 + P(15, 2) * _tmp25);
|
||||
_kz(16, 0) = _tmp33 * (P(16, 1) * _tmp27 + P(16, 2) * _tmp25);
|
||||
_kz(17, 0) = _tmp33 * (P(17, 1) * _tmp27 + P(17, 2) * _tmp25);
|
||||
_kz(18, 0) = _tmp33 * (P(18, 1) * _tmp27 + P(18, 2) * _tmp25);
|
||||
_kz(19, 0) = _tmp33 * (P(19, 1) * _tmp27 + P(19, 2) * _tmp25);
|
||||
_kz(20, 0) = _tmp33 * (P(20, 1) * _tmp27 + P(20, 2) * _tmp25);
|
||||
_kz(21, 0) = _tmp33 * (P(21, 1) * _tmp27 + P(21, 2) * _tmp25);
|
||||
_kz(22, 0) = _tmp33 * (P(22, 1) * _tmp27 + P(22, 2) * _tmp25);
|
||||
_kz(23, 0) = _tmp33 * (P(23, 1) * _tmp27 + P(23, 2) * _tmp25);
|
||||
_kz(0, 0) = _tmp30 * (P(0, 1) * _tmp24 + _tmp26);
|
||||
_kz(1, 0) = _tmp30 * (P(1, 0) * _tmp23 + _tmp25);
|
||||
_kz(2, 0) = _tmp30 * (P(2, 0) * _tmp23 + P(2, 1) * _tmp24);
|
||||
_kz(3, 0) = _tmp30 * (P(3, 0) * _tmp23 + P(3, 1) * _tmp24);
|
||||
_kz(4, 0) = _tmp30 * (P(4, 0) * _tmp23 + P(4, 1) * _tmp24);
|
||||
_kz(5, 0) = _tmp30 * (P(5, 0) * _tmp23 + P(5, 1) * _tmp24);
|
||||
_kz(6, 0) = _tmp30 * (P(6, 0) * _tmp23 + P(6, 1) * _tmp24);
|
||||
_kz(7, 0) = _tmp30 * (P(7, 0) * _tmp23 + P(7, 1) * _tmp24);
|
||||
_kz(8, 0) = _tmp30 * (P(8, 0) * _tmp23 + P(8, 1) * _tmp24);
|
||||
_kz(9, 0) = _tmp30 * (P(9, 0) * _tmp23 + P(9, 1) * _tmp24);
|
||||
_kz(10, 0) = _tmp30 * (P(10, 0) * _tmp23 + P(10, 1) * _tmp24);
|
||||
_kz(11, 0) = _tmp30 * (P(11, 0) * _tmp23 + P(11, 1) * _tmp24);
|
||||
_kz(12, 0) = _tmp30 * (P(12, 0) * _tmp23 + P(12, 1) * _tmp24);
|
||||
_kz(13, 0) = _tmp30 * (P(13, 0) * _tmp23 + P(13, 1) * _tmp24);
|
||||
_kz(14, 0) = _tmp30 * (P(14, 0) * _tmp23 + P(14, 1) * _tmp24);
|
||||
_kz(15, 0) = _tmp30 * (P(15, 0) * _tmp23 + P(15, 1) * _tmp24);
|
||||
_kz(16, 0) = _tmp30 * (P(16, 0) * _tmp23 + P(16, 1) * _tmp24);
|
||||
_kz(17, 0) = _tmp30 * (P(17, 0) * _tmp23 + P(17, 1) * _tmp24);
|
||||
_kz(18, 0) = _tmp30 * (P(18, 0) * _tmp23 + P(18, 1) * _tmp24);
|
||||
_kz(19, 0) = _tmp30 * (P(19, 0) * _tmp23 + P(19, 1) * _tmp24);
|
||||
_kz(20, 0) = _tmp30 * (P(20, 0) * _tmp23 + P(20, 1) * _tmp24);
|
||||
_kz(21, 0) = _tmp30 * (P(21, 0) * _tmp23 + P(21, 1) * _tmp24);
|
||||
_kz(22, 0) = _tmp30 * (P(22, 0) * _tmp23 + P(22, 1) * _tmp24);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+9
-9
@@ -17,21 +17,21 @@ namespace sym {
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* P: Matrix23_23
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* pred: Scalar
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
* H: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const pred = nullptr,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
// Total ops: 22
|
||||
|
||||
// Input arrays
|
||||
@@ -54,17 +54,17 @@ void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>&
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R - _tmp2 * (-P(16, 16) * _tmp2 + P(17, 16) * _tmp3) +
|
||||
_tmp3 * (-P(16, 17) * _tmp2 + P(17, 17) * _tmp3);
|
||||
_innov_var = R - _tmp2 * (-P(15, 15) * _tmp2 + P(16, 15) * _tmp3) +
|
||||
_tmp3 * (-P(15, 16) * _tmp2 + P(16, 16) * _tmp3);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(16, 0) = -_tmp2;
|
||||
_h(17, 0) = _tmp3;
|
||||
_h(15, 0) = -_tmp2;
|
||||
_h(16, 0) = _tmp3;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+103
-121
@@ -17,7 +17,7 @@ namespace sym {
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* P: Matrix23_23
|
||||
* meas: Matrix31
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
@@ -25,155 +25,137 @@ namespace sym {
|
||||
* Outputs:
|
||||
* innov: Matrix31
|
||||
* innov_var: Matrix31
|
||||
* Hx: Matrix24_1
|
||||
* Hx: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P,
|
||||
const matrix::Matrix<Scalar, 3, 1>& meas, const Scalar R,
|
||||
const Scalar epsilon,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov = nullptr,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const Hx = nullptr) {
|
||||
// Total ops: 471
|
||||
matrix::Matrix<Scalar, 23, 1>* const Hx = nullptr) {
|
||||
// Total ops: 314
|
||||
|
||||
// Unused inputs
|
||||
(void)epsilon;
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (49)
|
||||
const Scalar _tmp0 = -2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = -2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp2 = _tmp0 + _tmp1 + 1;
|
||||
const Scalar _tmp3 = 2 * state(0, 0);
|
||||
const Scalar _tmp4 = _tmp3 * state(3, 0);
|
||||
const Scalar _tmp5 = 2 * state(2, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(1, 0);
|
||||
const Scalar _tmp7 = _tmp4 + _tmp6;
|
||||
const Scalar _tmp8 = _tmp5 * state(0, 0);
|
||||
const Scalar _tmp9 = 2 * state(1, 0);
|
||||
const Scalar _tmp10 = _tmp9 * state(3, 0);
|
||||
const Scalar _tmp11 = _tmp10 - _tmp8;
|
||||
const Scalar _tmp12 = 1 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp13 = _tmp0 + _tmp12;
|
||||
const Scalar _tmp14 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp15 = _tmp3 * state(1, 0);
|
||||
const Scalar _tmp16 = _tmp14 + _tmp15;
|
||||
const Scalar _tmp17 = -_tmp4 + _tmp6;
|
||||
const Scalar _tmp18 = _tmp1 + _tmp12;
|
||||
const Scalar _tmp19 = _tmp14 - _tmp15;
|
||||
const Scalar _tmp20 = _tmp10 + _tmp8;
|
||||
const Scalar _tmp21 = _tmp9 * state(18, 0);
|
||||
const Scalar _tmp22 = _tmp3 * state(17, 0);
|
||||
const Scalar _tmp23 = _tmp21 + _tmp22 - 4 * state(16, 0) * state(3, 0);
|
||||
const Scalar _tmp24 = 4 * state(2, 0);
|
||||
const Scalar _tmp25 = _tmp9 * state(17, 0);
|
||||
const Scalar _tmp26 = _tmp3 * state(18, 0);
|
||||
const Scalar _tmp27 = -_tmp24 * state(16, 0) + _tmp25 - _tmp26;
|
||||
const Scalar _tmp28 = state(17, 0) * state(3, 0);
|
||||
const Scalar _tmp29 = 2 * _tmp28;
|
||||
const Scalar _tmp30 = _tmp5 * state(18, 0);
|
||||
const Scalar _tmp31 = _tmp29 - _tmp30;
|
||||
const Scalar _tmp32 = 2 * state(3, 0);
|
||||
const Scalar _tmp33 = _tmp32 * state(18, 0);
|
||||
const Scalar _tmp34 = _tmp5 * state(17, 0);
|
||||
const Scalar _tmp35 = _tmp33 + _tmp34;
|
||||
const Scalar _tmp36 = _tmp5 * state(16, 0);
|
||||
const Scalar _tmp37 = 4 * state(1, 0);
|
||||
const Scalar _tmp38 = _tmp26 + _tmp36 - _tmp37 * state(17, 0);
|
||||
const Scalar _tmp39 = _tmp3 * state(16, 0);
|
||||
const Scalar _tmp40 = -4 * _tmp28 + _tmp30 - _tmp39;
|
||||
const Scalar _tmp41 = _tmp32 * state(16, 0);
|
||||
const Scalar _tmp42 = _tmp21 - _tmp41;
|
||||
const Scalar _tmp43 = _tmp9 * state(16, 0);
|
||||
const Scalar _tmp44 = _tmp33 + _tmp43;
|
||||
const Scalar _tmp45 = -_tmp22 - _tmp37 * state(18, 0) + _tmp41;
|
||||
const Scalar _tmp46 = -_tmp24 * state(18, 0) + _tmp29 + _tmp39;
|
||||
const Scalar _tmp47 = _tmp34 + _tmp43;
|
||||
const Scalar _tmp48 = -_tmp25 + _tmp36;
|
||||
// Intermediate terms (47)
|
||||
const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = -2 * _tmp0;
|
||||
const Scalar _tmp2 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp3 = -2 * _tmp2;
|
||||
const Scalar _tmp4 = _tmp1 + _tmp3 + 1;
|
||||
const Scalar _tmp5 = 2 * state(0, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp7 = 2 * state(1, 0);
|
||||
const Scalar _tmp8 = _tmp7 * state(2, 0);
|
||||
const Scalar _tmp9 = _tmp6 + _tmp8;
|
||||
const Scalar _tmp10 = _tmp5 * state(2, 0);
|
||||
const Scalar _tmp11 = -_tmp10;
|
||||
const Scalar _tmp12 = _tmp7 * state(3, 0);
|
||||
const Scalar _tmp13 = _tmp11 + _tmp12;
|
||||
const Scalar _tmp14 = _tmp13 * state(18, 0) + _tmp9 * state(17, 0);
|
||||
const Scalar _tmp15 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp16 = 1 - 2 * _tmp15;
|
||||
const Scalar _tmp17 = _tmp1 + _tmp16;
|
||||
const Scalar _tmp18 = 2 * state(2, 0) * state(3, 0);
|
||||
const Scalar _tmp19 = _tmp7 * state(0, 0);
|
||||
const Scalar _tmp20 = _tmp18 + _tmp19;
|
||||
const Scalar _tmp21 = -_tmp6;
|
||||
const Scalar _tmp22 = _tmp21 + _tmp8;
|
||||
const Scalar _tmp23 = _tmp20 * state(18, 0) + _tmp22 * state(16, 0);
|
||||
const Scalar _tmp24 = _tmp16 + _tmp3;
|
||||
const Scalar _tmp25 = -_tmp19;
|
||||
const Scalar _tmp26 = _tmp18 + _tmp25;
|
||||
const Scalar _tmp27 = _tmp10 + _tmp12;
|
||||
const Scalar _tmp28 = _tmp26 * state(17, 0) + _tmp27 * state(16, 0);
|
||||
const Scalar _tmp29 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp30 = -_tmp29;
|
||||
const Scalar _tmp31 = _tmp2 + _tmp30;
|
||||
const Scalar _tmp32 = -_tmp0;
|
||||
const Scalar _tmp33 = _tmp15 + _tmp32;
|
||||
const Scalar _tmp34 = -_tmp18;
|
||||
const Scalar _tmp35 = -_tmp12;
|
||||
const Scalar _tmp36 = state(16, 0) * (_tmp11 + _tmp35) + state(17, 0) * (_tmp19 + _tmp34) +
|
||||
state(18, 0) * (_tmp31 + _tmp33);
|
||||
const Scalar _tmp37 = -_tmp15;
|
||||
const Scalar _tmp38 = _tmp23 + state(17, 0) * (_tmp2 + _tmp29 + _tmp32 + _tmp37);
|
||||
const Scalar _tmp39 = _tmp0 + _tmp37;
|
||||
const Scalar _tmp40 = -_tmp8;
|
||||
const Scalar _tmp41 = state(16, 0) * (_tmp31 + _tmp39) + state(17, 0) * (_tmp21 + _tmp40) +
|
||||
state(18, 0) * (_tmp10 + _tmp35);
|
||||
const Scalar _tmp42 = -_tmp2;
|
||||
const Scalar _tmp43 = _tmp29 + _tmp42;
|
||||
const Scalar _tmp44 = _tmp28 + state(18, 0) * (_tmp39 + _tmp43);
|
||||
const Scalar _tmp45 = _tmp14 + state(16, 0) * (_tmp33 + _tmp43);
|
||||
const Scalar _tmp46 = state(16, 0) * (_tmp40 + _tmp6) +
|
||||
state(17, 0) * (_tmp0 + _tmp15 + _tmp30 + _tmp42) +
|
||||
state(18, 0) * (_tmp25 + _tmp34);
|
||||
|
||||
// Output terms (3)
|
||||
if (innov != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _innov = (*innov);
|
||||
|
||||
_innov(0, 0) = _tmp11 * state(18, 0) + _tmp2 * state(16, 0) + _tmp7 * state(17, 0) -
|
||||
meas(0, 0) + state(19, 0);
|
||||
_innov(1, 0) = _tmp13 * state(17, 0) + _tmp16 * state(18, 0) + _tmp17 * state(16, 0) -
|
||||
meas(1, 0) + state(20, 0);
|
||||
_innov(2, 0) = _tmp18 * state(18, 0) + _tmp19 * state(17, 0) + _tmp20 * state(16, 0) -
|
||||
meas(2, 0) + state(21, 0);
|
||||
_innov(0, 0) = _tmp14 + _tmp4 * state(16, 0) - meas(0, 0) + state(19, 0);
|
||||
_innov(1, 0) = _tmp17 * state(17, 0) + _tmp23 - meas(1, 0) + state(20, 0);
|
||||
_innov(2, 0) = _tmp24 * state(18, 0) + _tmp28 - meas(2, 0) + state(21, 0);
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var(0, 0) =
|
||||
P(0, 19) * _tmp31 + P(1, 19) * _tmp35 + P(16, 19) * _tmp2 + P(17, 19) * _tmp7 +
|
||||
P(18, 19) * _tmp11 + P(19, 19) + P(2, 19) * _tmp27 + P(3, 19) * _tmp23 + R +
|
||||
_tmp11 * (P(0, 18) * _tmp31 + P(1, 18) * _tmp35 + P(16, 18) * _tmp2 + P(17, 18) * _tmp7 +
|
||||
P(18, 18) * _tmp11 + P(19, 18) + P(2, 18) * _tmp27 + P(3, 18) * _tmp23) +
|
||||
_tmp2 * (P(0, 16) * _tmp31 + P(1, 16) * _tmp35 + P(16, 16) * _tmp2 + P(17, 16) * _tmp7 +
|
||||
P(18, 16) * _tmp11 + P(19, 16) + P(2, 16) * _tmp27 + P(3, 16) * _tmp23) +
|
||||
_tmp23 * (P(0, 3) * _tmp31 + P(1, 3) * _tmp35 + P(16, 3) * _tmp2 + P(17, 3) * _tmp7 +
|
||||
P(18, 3) * _tmp11 + P(19, 3) + P(2, 3) * _tmp27 + P(3, 3) * _tmp23) +
|
||||
_tmp27 * (P(0, 2) * _tmp31 + P(1, 2) * _tmp35 + P(16, 2) * _tmp2 + P(17, 2) * _tmp7 +
|
||||
P(18, 2) * _tmp11 + P(19, 2) + P(2, 2) * _tmp27 + P(3, 2) * _tmp23) +
|
||||
_tmp31 * (P(0, 0) * _tmp31 + P(1, 0) * _tmp35 + P(16, 0) * _tmp2 + P(17, 0) * _tmp7 +
|
||||
P(18, 0) * _tmp11 + P(19, 0) + P(2, 0) * _tmp27 + P(3, 0) * _tmp23) +
|
||||
_tmp35 * (P(0, 1) * _tmp31 + P(1, 1) * _tmp35 + P(16, 1) * _tmp2 + P(17, 1) * _tmp7 +
|
||||
P(18, 1) * _tmp11 + P(19, 1) + P(2, 1) * _tmp27 + P(3, 1) * _tmp23) +
|
||||
_tmp7 * (P(0, 17) * _tmp31 + P(1, 17) * _tmp35 + P(16, 17) * _tmp2 + P(17, 17) * _tmp7 +
|
||||
P(18, 17) * _tmp11 + P(19, 17) + P(2, 17) * _tmp27 + P(3, 17) * _tmp23);
|
||||
_innov_var(1, 0) =
|
||||
P(0, 20) * _tmp42 + P(1, 20) * _tmp38 + P(16, 20) * _tmp17 + P(17, 20) * _tmp13 +
|
||||
P(18, 20) * _tmp16 + P(2, 20) * _tmp44 + P(20, 20) + P(3, 20) * _tmp40 + R +
|
||||
_tmp13 * (P(0, 17) * _tmp42 + P(1, 17) * _tmp38 + P(16, 17) * _tmp17 + P(17, 17) * _tmp13 +
|
||||
P(18, 17) * _tmp16 + P(2, 17) * _tmp44 + P(20, 17) + P(3, 17) * _tmp40) +
|
||||
_tmp16 * (P(0, 18) * _tmp42 + P(1, 18) * _tmp38 + P(16, 18) * _tmp17 + P(17, 18) * _tmp13 +
|
||||
P(18, 18) * _tmp16 + P(2, 18) * _tmp44 + P(20, 18) + P(3, 18) * _tmp40) +
|
||||
_tmp17 * (P(0, 16) * _tmp42 + P(1, 16) * _tmp38 + P(16, 16) * _tmp17 + P(17, 16) * _tmp13 +
|
||||
P(18, 16) * _tmp16 + P(2, 16) * _tmp44 + P(20, 16) + P(3, 16) * _tmp40) +
|
||||
_tmp38 * (P(0, 1) * _tmp42 + P(1, 1) * _tmp38 + P(16, 1) * _tmp17 + P(17, 1) * _tmp13 +
|
||||
P(18, 1) * _tmp16 + P(2, 1) * _tmp44 + P(20, 1) + P(3, 1) * _tmp40) +
|
||||
_tmp40 * (P(0, 3) * _tmp42 + P(1, 3) * _tmp38 + P(16, 3) * _tmp17 + P(17, 3) * _tmp13 +
|
||||
P(18, 3) * _tmp16 + P(2, 3) * _tmp44 + P(20, 3) + P(3, 3) * _tmp40) +
|
||||
_tmp42 * (P(0, 0) * _tmp42 + P(1, 0) * _tmp38 + P(16, 0) * _tmp17 + P(17, 0) * _tmp13 +
|
||||
P(18, 0) * _tmp16 + P(2, 0) * _tmp44 + P(20, 0) + P(3, 0) * _tmp40) +
|
||||
_tmp44 * (P(0, 2) * _tmp42 + P(1, 2) * _tmp38 + P(16, 2) * _tmp17 + P(17, 2) * _tmp13 +
|
||||
P(18, 2) * _tmp16 + P(2, 2) * _tmp44 + P(20, 2) + P(3, 2) * _tmp40);
|
||||
_innov_var(2, 0) =
|
||||
P(0, 21) * _tmp48 + P(1, 21) * _tmp45 + P(16, 21) * _tmp20 + P(17, 21) * _tmp19 +
|
||||
P(18, 21) * _tmp18 + P(2, 21) * _tmp46 + P(21, 21) + P(3, 21) * _tmp47 + R +
|
||||
_tmp18 * (P(0, 18) * _tmp48 + P(1, 18) * _tmp45 + P(16, 18) * _tmp20 + P(17, 18) * _tmp19 +
|
||||
P(18, 18) * _tmp18 + P(2, 18) * _tmp46 + P(21, 18) + P(3, 18) * _tmp47) +
|
||||
_tmp19 * (P(0, 17) * _tmp48 + P(1, 17) * _tmp45 + P(16, 17) * _tmp20 + P(17, 17) * _tmp19 +
|
||||
P(18, 17) * _tmp18 + P(2, 17) * _tmp46 + P(21, 17) + P(3, 17) * _tmp47) +
|
||||
_tmp20 * (P(0, 16) * _tmp48 + P(1, 16) * _tmp45 + P(16, 16) * _tmp20 + P(17, 16) * _tmp19 +
|
||||
P(18, 16) * _tmp18 + P(2, 16) * _tmp46 + P(21, 16) + P(3, 16) * _tmp47) +
|
||||
_tmp45 * (P(0, 1) * _tmp48 + P(1, 1) * _tmp45 + P(16, 1) * _tmp20 + P(17, 1) * _tmp19 +
|
||||
P(18, 1) * _tmp18 + P(2, 1) * _tmp46 + P(21, 1) + P(3, 1) * _tmp47) +
|
||||
_tmp46 * (P(0, 2) * _tmp48 + P(1, 2) * _tmp45 + P(16, 2) * _tmp20 + P(17, 2) * _tmp19 +
|
||||
P(18, 2) * _tmp18 + P(2, 2) * _tmp46 + P(21, 2) + P(3, 2) * _tmp47) +
|
||||
_tmp47 * (P(0, 3) * _tmp48 + P(1, 3) * _tmp45 + P(16, 3) * _tmp20 + P(17, 3) * _tmp19 +
|
||||
P(18, 3) * _tmp18 + P(2, 3) * _tmp46 + P(21, 3) + P(3, 3) * _tmp47) +
|
||||
_tmp48 * (P(0, 0) * _tmp48 + P(1, 0) * _tmp45 + P(16, 0) * _tmp20 + P(17, 0) * _tmp19 +
|
||||
P(18, 0) * _tmp18 + P(2, 0) * _tmp46 + P(21, 0) + P(3, 0) * _tmp47);
|
||||
_innov_var(0, 0) = P(1, 18) * _tmp36 + P(15, 18) * _tmp4 + P(16, 18) * _tmp9 +
|
||||
P(17, 18) * _tmp13 + P(18, 18) + P(2, 18) * _tmp38 + R +
|
||||
_tmp13 * (P(1, 17) * _tmp36 + P(15, 17) * _tmp4 + P(16, 17) * _tmp9 +
|
||||
P(17, 17) * _tmp13 + P(18, 17) + P(2, 17) * _tmp38) +
|
||||
_tmp36 * (P(1, 1) * _tmp36 + P(15, 1) * _tmp4 + P(16, 1) * _tmp9 +
|
||||
P(17, 1) * _tmp13 + P(18, 1) + P(2, 1) * _tmp38) +
|
||||
_tmp38 * (P(1, 2) * _tmp36 + P(15, 2) * _tmp4 + P(16, 2) * _tmp9 +
|
||||
P(17, 2) * _tmp13 + P(18, 2) + P(2, 2) * _tmp38) +
|
||||
_tmp4 * (P(1, 15) * _tmp36 + P(15, 15) * _tmp4 + P(16, 15) * _tmp9 +
|
||||
P(17, 15) * _tmp13 + P(18, 15) + P(2, 15) * _tmp38) +
|
||||
_tmp9 * (P(1, 16) * _tmp36 + P(15, 16) * _tmp4 + P(16, 16) * _tmp9 +
|
||||
P(17, 16) * _tmp13 + P(18, 16) + P(2, 16) * _tmp38);
|
||||
_innov_var(1, 0) = P(0, 19) * _tmp44 + P(15, 19) * _tmp22 + P(16, 19) * _tmp17 +
|
||||
P(17, 19) * _tmp20 + P(19, 19) + P(2, 19) * _tmp41 + R +
|
||||
_tmp17 * (P(0, 16) * _tmp44 + P(15, 16) * _tmp22 + P(16, 16) * _tmp17 +
|
||||
P(17, 16) * _tmp20 + P(19, 16) + P(2, 16) * _tmp41) +
|
||||
_tmp20 * (P(0, 17) * _tmp44 + P(15, 17) * _tmp22 + P(16, 17) * _tmp17 +
|
||||
P(17, 17) * _tmp20 + P(19, 17) + P(2, 17) * _tmp41) +
|
||||
_tmp22 * (P(0, 15) * _tmp44 + P(15, 15) * _tmp22 + P(16, 15) * _tmp17 +
|
||||
P(17, 15) * _tmp20 + P(19, 15) + P(2, 15) * _tmp41) +
|
||||
_tmp41 * (P(0, 2) * _tmp44 + P(15, 2) * _tmp22 + P(16, 2) * _tmp17 +
|
||||
P(17, 2) * _tmp20 + P(19, 2) + P(2, 2) * _tmp41) +
|
||||
_tmp44 * (P(0, 0) * _tmp44 + P(15, 0) * _tmp22 + P(16, 0) * _tmp17 +
|
||||
P(17, 0) * _tmp20 + P(19, 0) + P(2, 0) * _tmp41);
|
||||
_innov_var(2, 0) = P(0, 20) * _tmp46 + P(1, 20) * _tmp45 + P(15, 20) * _tmp27 +
|
||||
P(16, 20) * _tmp26 + P(17, 20) * _tmp24 + P(20, 20) + R +
|
||||
_tmp24 * (P(0, 17) * _tmp46 + P(1, 17) * _tmp45 + P(15, 17) * _tmp27 +
|
||||
P(16, 17) * _tmp26 + P(17, 17) * _tmp24 + P(20, 17)) +
|
||||
_tmp26 * (P(0, 16) * _tmp46 + P(1, 16) * _tmp45 + P(15, 16) * _tmp27 +
|
||||
P(16, 16) * _tmp26 + P(17, 16) * _tmp24 + P(20, 16)) +
|
||||
_tmp27 * (P(0, 15) * _tmp46 + P(1, 15) * _tmp45 + P(15, 15) * _tmp27 +
|
||||
P(16, 15) * _tmp26 + P(17, 15) * _tmp24 + P(20, 15)) +
|
||||
_tmp45 * (P(0, 1) * _tmp46 + P(1, 1) * _tmp45 + P(15, 1) * _tmp27 +
|
||||
P(16, 1) * _tmp26 + P(17, 1) * _tmp24 + P(20, 1)) +
|
||||
_tmp46 * (P(0, 0) * _tmp46 + P(1, 0) * _tmp45 + P(15, 0) * _tmp27 +
|
||||
P(16, 0) * _tmp26 + P(17, 0) * _tmp24 + P(20, 0));
|
||||
}
|
||||
|
||||
if (Hx != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _hx = (*Hx);
|
||||
matrix::Matrix<Scalar, 23, 1>& _hx = (*Hx);
|
||||
|
||||
_hx.setZero();
|
||||
|
||||
_hx(0, 0) = _tmp31;
|
||||
_hx(1, 0) = _tmp35;
|
||||
_hx(2, 0) = _tmp27;
|
||||
_hx(3, 0) = _tmp23;
|
||||
_hx(16, 0) = _tmp2;
|
||||
_hx(17, 0) = _tmp7;
|
||||
_hx(18, 0) = _tmp11;
|
||||
_hx(19, 0) = 1;
|
||||
_hx(1, 0) = _tmp36;
|
||||
_hx(2, 0) = _tmp38;
|
||||
_hx(15, 0) = _tmp4;
|
||||
_hx(16, 0) = _tmp9;
|
||||
_hx(17, 0) = _tmp13;
|
||||
_hx(18, 0) = 1;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+44
-44
@@ -17,77 +17,77 @@ namespace sym {
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* P: Matrix23_23
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
* H: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 160
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
// Total ops: 110
|
||||
|
||||
// Unused inputs
|
||||
(void)epsilon;
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (12)
|
||||
const Scalar _tmp0 = 2 * state(16, 0);
|
||||
const Scalar _tmp1 = 4 * state(17, 0);
|
||||
const Scalar _tmp2 = 2 * state(18, 0);
|
||||
const Scalar _tmp3 = _tmp0 * state(2, 0) - _tmp1 * state(1, 0) + _tmp2 * state(0, 0);
|
||||
const Scalar _tmp4 = -_tmp0 * state(0, 0) - _tmp1 * state(3, 0) + _tmp2 * state(2, 0);
|
||||
const Scalar _tmp5 = -_tmp0 * state(3, 0) + _tmp2 * state(1, 0);
|
||||
const Scalar _tmp6 = _tmp0 * state(1, 0) + _tmp2 * state(3, 0);
|
||||
const Scalar _tmp7 =
|
||||
-2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
|
||||
const Scalar _tmp8 = 2 * state(3, 0);
|
||||
const Scalar _tmp9 = 2 * state(1, 0);
|
||||
const Scalar _tmp10 = _tmp8 * state(2, 0) + _tmp9 * state(0, 0);
|
||||
const Scalar _tmp11 = -_tmp8 * state(0, 0) + _tmp9 * state(2, 0);
|
||||
// Intermediate terms (18)
|
||||
const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp2 = -2 * _tmp0 - 2 * _tmp1 + 1;
|
||||
const Scalar _tmp3 = 2 * state(2, 0);
|
||||
const Scalar _tmp4 = _tmp3 * state(3, 0);
|
||||
const Scalar _tmp5 = 2 * state(0, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(1, 0);
|
||||
const Scalar _tmp7 = _tmp4 + _tmp6;
|
||||
const Scalar _tmp8 = -_tmp5 * state(3, 0);
|
||||
const Scalar _tmp9 = _tmp3 * state(1, 0);
|
||||
const Scalar _tmp10 = _tmp8 + _tmp9;
|
||||
const Scalar _tmp11 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp12 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp13 = _tmp0 - _tmp1;
|
||||
const Scalar _tmp14 = _tmp3 * state(0, 0);
|
||||
const Scalar _tmp15 = 2 * state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp16 = state(16, 0) * (-_tmp11 + _tmp12 + _tmp13) +
|
||||
state(17, 0) * (_tmp8 - _tmp9) + state(18, 0) * (_tmp14 - _tmp15);
|
||||
const Scalar _tmp17 = state(16, 0) * (_tmp14 + _tmp15) + state(17, 0) * (_tmp4 - _tmp6) +
|
||||
state(18, 0) * (_tmp11 - _tmp12 + _tmp13);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var =
|
||||
P(0, 20) * _tmp5 + P(1, 20) * _tmp3 + P(16, 20) * _tmp11 + P(17, 20) * _tmp7 +
|
||||
P(18, 20) * _tmp10 + P(2, 20) * _tmp6 + P(20, 20) + P(3, 20) * _tmp4 + R +
|
||||
_tmp10 * (P(0, 18) * _tmp5 + P(1, 18) * _tmp3 + P(16, 18) * _tmp11 + P(17, 18) * _tmp7 +
|
||||
P(18, 18) * _tmp10 + P(2, 18) * _tmp6 + P(20, 18) + P(3, 18) * _tmp4) +
|
||||
_tmp11 * (P(0, 16) * _tmp5 + P(1, 16) * _tmp3 + P(16, 16) * _tmp11 + P(17, 16) * _tmp7 +
|
||||
P(18, 16) * _tmp10 + P(2, 16) * _tmp6 + P(20, 16) + P(3, 16) * _tmp4) +
|
||||
_tmp3 * (P(0, 1) * _tmp5 + P(1, 1) * _tmp3 + P(16, 1) * _tmp11 + P(17, 1) * _tmp7 +
|
||||
P(18, 1) * _tmp10 + P(2, 1) * _tmp6 + P(20, 1) + P(3, 1) * _tmp4) +
|
||||
_tmp4 * (P(0, 3) * _tmp5 + P(1, 3) * _tmp3 + P(16, 3) * _tmp11 + P(17, 3) * _tmp7 +
|
||||
P(18, 3) * _tmp10 + P(2, 3) * _tmp6 + P(20, 3) + P(3, 3) * _tmp4) +
|
||||
_tmp5 * (P(0, 0) * _tmp5 + P(1, 0) * _tmp3 + P(16, 0) * _tmp11 + P(17, 0) * _tmp7 +
|
||||
P(18, 0) * _tmp10 + P(2, 0) * _tmp6 + P(20, 0) + P(3, 0) * _tmp4) +
|
||||
_tmp6 * (P(0, 2) * _tmp5 + P(1, 2) * _tmp3 + P(16, 2) * _tmp11 + P(17, 2) * _tmp7 +
|
||||
P(18, 2) * _tmp10 + P(2, 2) * _tmp6 + P(20, 2) + P(3, 2) * _tmp4) +
|
||||
_tmp7 * (P(0, 17) * _tmp5 + P(1, 17) * _tmp3 + P(16, 17) * _tmp11 + P(17, 17) * _tmp7 +
|
||||
P(18, 17) * _tmp10 + P(2, 17) * _tmp6 + P(20, 17) + P(3, 17) * _tmp4);
|
||||
_innov_var = P(0, 19) * _tmp17 + P(15, 19) * _tmp10 + P(16, 19) * _tmp2 + P(17, 19) * _tmp7 +
|
||||
P(19, 19) + P(2, 19) * _tmp16 + R +
|
||||
_tmp10 * (P(0, 15) * _tmp17 + P(15, 15) * _tmp10 + P(16, 15) * _tmp2 +
|
||||
P(17, 15) * _tmp7 + P(19, 15) + P(2, 15) * _tmp16) +
|
||||
_tmp16 * (P(0, 2) * _tmp17 + P(15, 2) * _tmp10 + P(16, 2) * _tmp2 +
|
||||
P(17, 2) * _tmp7 + P(19, 2) + P(2, 2) * _tmp16) +
|
||||
_tmp17 * (P(0, 0) * _tmp17 + P(15, 0) * _tmp10 + P(16, 0) * _tmp2 +
|
||||
P(17, 0) * _tmp7 + P(19, 0) + P(2, 0) * _tmp16) +
|
||||
_tmp2 * (P(0, 16) * _tmp17 + P(15, 16) * _tmp10 + P(16, 16) * _tmp2 +
|
||||
P(17, 16) * _tmp7 + P(19, 16) + P(2, 16) * _tmp16) +
|
||||
_tmp7 * (P(0, 17) * _tmp17 + P(15, 17) * _tmp10 + P(16, 17) * _tmp2 +
|
||||
P(17, 17) * _tmp7 + P(19, 17) + P(2, 17) * _tmp16);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp5;
|
||||
_h(1, 0) = _tmp3;
|
||||
_h(2, 0) = _tmp6;
|
||||
_h(3, 0) = _tmp4;
|
||||
_h(16, 0) = _tmp11;
|
||||
_h(0, 0) = _tmp17;
|
||||
_h(2, 0) = _tmp16;
|
||||
_h(15, 0) = _tmp10;
|
||||
_h(16, 0) = _tmp2;
|
||||
_h(17, 0) = _tmp7;
|
||||
_h(18, 0) = _tmp10;
|
||||
_h(20, 0) = 1;
|
||||
_h(19, 0) = 1;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+45
-45
@@ -17,77 +17,77 @@ namespace sym {
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* P: Matrix23_23
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
* H: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 160
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
// Total ops: 110
|
||||
|
||||
// Unused inputs
|
||||
(void)epsilon;
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (12)
|
||||
const Scalar _tmp0 = 2 * state(16, 0);
|
||||
const Scalar _tmp1 = 4 * state(18, 0);
|
||||
const Scalar _tmp2 = 2 * state(17, 0);
|
||||
const Scalar _tmp3 = _tmp0 * state(3, 0) - _tmp1 * state(1, 0) - _tmp2 * state(0, 0);
|
||||
const Scalar _tmp4 = _tmp0 * state(0, 0) - _tmp1 * state(2, 0) + _tmp2 * state(3, 0);
|
||||
const Scalar _tmp5 = _tmp0 * state(1, 0) + _tmp2 * state(2, 0);
|
||||
const Scalar _tmp6 = _tmp0 * state(2, 0) - _tmp2 * state(1, 0);
|
||||
const Scalar _tmp7 = 2 * state(2, 0);
|
||||
const Scalar _tmp8 = 2 * state(1, 0);
|
||||
const Scalar _tmp9 = _tmp7 * state(0, 0) + _tmp8 * state(3, 0);
|
||||
const Scalar _tmp10 = _tmp7 * state(3, 0) - _tmp8 * state(0, 0);
|
||||
const Scalar _tmp11 =
|
||||
-2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1;
|
||||
// Intermediate terms (18)
|
||||
const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp2 = -2 * _tmp0 - 2 * _tmp1 + 1;
|
||||
const Scalar _tmp3 = 2 * state(2, 0);
|
||||
const Scalar _tmp4 = _tmp3 * state(3, 0);
|
||||
const Scalar _tmp5 = 2 * state(0, 0);
|
||||
const Scalar _tmp6 = -_tmp5 * state(1, 0);
|
||||
const Scalar _tmp7 = _tmp4 + _tmp6;
|
||||
const Scalar _tmp8 = _tmp3 * state(0, 0);
|
||||
const Scalar _tmp9 = 2 * state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp10 = _tmp8 + _tmp9;
|
||||
const Scalar _tmp11 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp12 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp13 = -_tmp0 + _tmp1;
|
||||
const Scalar _tmp14 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp15 = _tmp3 * state(1, 0);
|
||||
const Scalar _tmp16 = state(16, 0) * (-_tmp11 + _tmp12 + _tmp13) +
|
||||
state(17, 0) * (_tmp14 + _tmp15) + state(18, 0) * (-_tmp8 + _tmp9);
|
||||
const Scalar _tmp17 = state(16, 0) * (_tmp14 - _tmp15) +
|
||||
state(17, 0) * (_tmp11 - _tmp12 + _tmp13) + state(18, 0) * (-_tmp4 + _tmp6);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var =
|
||||
P(0, 21) * _tmp6 + P(1, 21) * _tmp3 + P(16, 21) * _tmp9 + P(17, 21) * _tmp10 +
|
||||
P(18, 21) * _tmp11 + P(2, 21) * _tmp4 + P(21, 21) + P(3, 21) * _tmp5 + R +
|
||||
_tmp10 * (P(0, 17) * _tmp6 + P(1, 17) * _tmp3 + P(16, 17) * _tmp9 + P(17, 17) * _tmp10 +
|
||||
P(18, 17) * _tmp11 + P(2, 17) * _tmp4 + P(21, 17) + P(3, 17) * _tmp5) +
|
||||
_tmp11 * (P(0, 18) * _tmp6 + P(1, 18) * _tmp3 + P(16, 18) * _tmp9 + P(17, 18) * _tmp10 +
|
||||
P(18, 18) * _tmp11 + P(2, 18) * _tmp4 + P(21, 18) + P(3, 18) * _tmp5) +
|
||||
_tmp3 * (P(0, 1) * _tmp6 + P(1, 1) * _tmp3 + P(16, 1) * _tmp9 + P(17, 1) * _tmp10 +
|
||||
P(18, 1) * _tmp11 + P(2, 1) * _tmp4 + P(21, 1) + P(3, 1) * _tmp5) +
|
||||
_tmp4 * (P(0, 2) * _tmp6 + P(1, 2) * _tmp3 + P(16, 2) * _tmp9 + P(17, 2) * _tmp10 +
|
||||
P(18, 2) * _tmp11 + P(2, 2) * _tmp4 + P(21, 2) + P(3, 2) * _tmp5) +
|
||||
_tmp5 * (P(0, 3) * _tmp6 + P(1, 3) * _tmp3 + P(16, 3) * _tmp9 + P(17, 3) * _tmp10 +
|
||||
P(18, 3) * _tmp11 + P(2, 3) * _tmp4 + P(21, 3) + P(3, 3) * _tmp5) +
|
||||
_tmp6 * (P(0, 0) * _tmp6 + P(1, 0) * _tmp3 + P(16, 0) * _tmp9 + P(17, 0) * _tmp10 +
|
||||
P(18, 0) * _tmp11 + P(2, 0) * _tmp4 + P(21, 0) + P(3, 0) * _tmp5) +
|
||||
_tmp9 * (P(0, 16) * _tmp6 + P(1, 16) * _tmp3 + P(16, 16) * _tmp9 + P(17, 16) * _tmp10 +
|
||||
P(18, 16) * _tmp11 + P(2, 16) * _tmp4 + P(21, 16) + P(3, 16) * _tmp5);
|
||||
_innov_var = P(0, 20) * _tmp17 + P(1, 20) * _tmp16 + P(15, 20) * _tmp10 + P(16, 20) * _tmp7 +
|
||||
P(17, 20) * _tmp2 + P(20, 20) + R +
|
||||
_tmp10 * (P(0, 15) * _tmp17 + P(1, 15) * _tmp16 + P(15, 15) * _tmp10 +
|
||||
P(16, 15) * _tmp7 + P(17, 15) * _tmp2 + P(20, 15)) +
|
||||
_tmp16 * (P(0, 1) * _tmp17 + P(1, 1) * _tmp16 + P(15, 1) * _tmp10 +
|
||||
P(16, 1) * _tmp7 + P(17, 1) * _tmp2 + P(20, 1)) +
|
||||
_tmp17 * (P(0, 0) * _tmp17 + P(1, 0) * _tmp16 + P(15, 0) * _tmp10 +
|
||||
P(16, 0) * _tmp7 + P(17, 0) * _tmp2 + P(20, 0)) +
|
||||
_tmp2 * (P(0, 17) * _tmp17 + P(1, 17) * _tmp16 + P(15, 17) * _tmp10 +
|
||||
P(16, 17) * _tmp7 + P(17, 17) * _tmp2 + P(20, 17)) +
|
||||
_tmp7 * (P(0, 16) * _tmp17 + P(1, 16) * _tmp16 + P(15, 16) * _tmp10 +
|
||||
P(16, 16) * _tmp7 + P(17, 16) * _tmp2 + P(20, 16));
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp6;
|
||||
_h(1, 0) = _tmp3;
|
||||
_h(2, 0) = _tmp4;
|
||||
_h(3, 0) = _tmp5;
|
||||
_h(16, 0) = _tmp9;
|
||||
_h(17, 0) = _tmp10;
|
||||
_h(18, 0) = _tmp11;
|
||||
_h(21, 0) = 1;
|
||||
_h(0, 0) = _tmp17;
|
||||
_h(1, 0) = _tmp16;
|
||||
_h(15, 0) = _tmp10;
|
||||
_h(16, 0) = _tmp7;
|
||||
_h(17, 0) = _tmp2;
|
||||
_h(20, 0) = 1;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
@@ -17,162 +17,165 @@ namespace sym {
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* P: Matrix23_23
|
||||
* innov_var: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* H: Matrix24_1
|
||||
* K: Matrix24_1
|
||||
* H: Matrix23_1
|
||||
* K: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
|
||||
const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
|
||||
// Total ops: 533
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar innov_var,
|
||||
const Scalar epsilon, matrix::Matrix<Scalar, 23, 1>* const H = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const K = nullptr) {
|
||||
// Total ops: 469
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (40)
|
||||
const Scalar _tmp0 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp1 = 2 * state(3, 0);
|
||||
const Scalar _tmp2 = 2 * state(6, 0);
|
||||
const Scalar _tmp3 = _tmp2 * state(2, 0);
|
||||
const Scalar _tmp4 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp5 = _tmp4 - 2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp6 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp7 = _tmp1 * state(0, 0);
|
||||
const Scalar _tmp8 = 2 * state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp9 = _tmp7 + _tmp8;
|
||||
const Scalar _tmp10 = 2 * state(0, 0);
|
||||
const Scalar _tmp11 = _tmp1 * state(1, 0) - _tmp10 * state(2, 0);
|
||||
const Scalar _tmp12 = _tmp0 * _tmp9 + _tmp11 * state(6, 0) + _tmp5 * _tmp6;
|
||||
const Scalar _tmp13 =
|
||||
_tmp12 + epsilon * (2 * math::min<Scalar>(0, (((_tmp12) > 0) - ((_tmp12) < 0))) + 1);
|
||||
const Scalar _tmp14 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp15 = -_tmp7 + _tmp8;
|
||||
const Scalar _tmp16 = _tmp1 * state(2, 0) + _tmp10 * state(1, 0);
|
||||
const Scalar _tmp17 =
|
||||
(_tmp0 * _tmp14 + _tmp15 * _tmp6 + _tmp16 * state(6, 0)) / std::pow(_tmp13, Scalar(2));
|
||||
const Scalar _tmp18 = _tmp2 * state(1, 0);
|
||||
const Scalar _tmp19 = Scalar(1.0) / (_tmp13);
|
||||
const Scalar _tmp20 = -_tmp17 * (_tmp0 * _tmp1 - _tmp3) + _tmp19 * (-_tmp1 * _tmp6 + _tmp18);
|
||||
const Scalar _tmp21 = 2 * _tmp0;
|
||||
const Scalar _tmp22 = _tmp1 * state(6, 0);
|
||||
const Scalar _tmp23 = 4 * _tmp0;
|
||||
const Scalar _tmp24 = 2 * _tmp6;
|
||||
const Scalar _tmp25 = _tmp2 * state(0, 0);
|
||||
const Scalar _tmp26 = -_tmp17 * (_tmp21 * state(2, 0) + _tmp22) +
|
||||
_tmp19 * (-_tmp23 * state(1, 0) + _tmp24 * state(2, 0) + _tmp25);
|
||||
const Scalar _tmp27 = 4 * _tmp6;
|
||||
const Scalar _tmp28 = -_tmp17 * (_tmp21 * state(1, 0) - _tmp25 - _tmp27 * state(2, 0)) +
|
||||
_tmp19 * (_tmp22 + _tmp24 * state(1, 0));
|
||||
const Scalar _tmp29 = -_tmp17 * (_tmp18 + _tmp21 * state(0, 0) - _tmp27 * state(3, 0)) +
|
||||
_tmp19 * (-_tmp23 * state(3, 0) - _tmp24 * state(0, 0) + _tmp3);
|
||||
const Scalar _tmp30 = _tmp17 * _tmp5;
|
||||
const Scalar _tmp31 = _tmp15 * _tmp19;
|
||||
const Scalar _tmp32 = -_tmp30 + _tmp31;
|
||||
const Scalar _tmp33 = _tmp17 * _tmp9;
|
||||
const Scalar _tmp34 = _tmp14 * _tmp19;
|
||||
const Scalar _tmp35 = -_tmp33 + _tmp34;
|
||||
const Scalar _tmp36 = -_tmp11 * _tmp17 + _tmp16 * _tmp19;
|
||||
const Scalar _tmp37 = _tmp30 - _tmp31;
|
||||
const Scalar _tmp38 = _tmp33 - _tmp34;
|
||||
const Scalar _tmp39 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
|
||||
// Intermediate terms (46)
|
||||
const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp2 = 1 - 2 * _tmp1;
|
||||
const Scalar _tmp3 = -2 * _tmp0 + _tmp2;
|
||||
const Scalar _tmp4 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp5 = 2 * state(0, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp7 = 2 * state(2, 0);
|
||||
const Scalar _tmp8 = _tmp7 * state(1, 0);
|
||||
const Scalar _tmp9 = _tmp6 + _tmp8;
|
||||
const Scalar _tmp10 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp11 = _tmp7 * state(0, 0);
|
||||
const Scalar _tmp12 = -_tmp11;
|
||||
const Scalar _tmp13 = 2 * state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp14 = _tmp12 + _tmp13;
|
||||
const Scalar _tmp15 = _tmp10 * _tmp9 + _tmp14 * state(6, 0) + _tmp3 * _tmp4;
|
||||
const Scalar _tmp16 =
|
||||
_tmp15 + epsilon * (2 * math::min<Scalar>(0, (((_tmp15) > 0) - ((_tmp15) < 0))) + 1);
|
||||
const Scalar _tmp17 = Scalar(1.0) / (_tmp16);
|
||||
const Scalar _tmp18 = _tmp7 * state(3, 0);
|
||||
const Scalar _tmp19 = _tmp5 * state(1, 0);
|
||||
const Scalar _tmp20 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp21 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp22 = -_tmp21;
|
||||
const Scalar _tmp23 = _tmp20 + _tmp22;
|
||||
const Scalar _tmp24 = _tmp17 * (_tmp10 * (_tmp18 - _tmp19) + _tmp4 * (_tmp11 + _tmp13) +
|
||||
state(6, 0) * (-_tmp0 + _tmp1 + _tmp23));
|
||||
const Scalar _tmp25 = -_tmp13;
|
||||
const Scalar _tmp26 = -_tmp20;
|
||||
const Scalar _tmp27 = _tmp0 - _tmp1;
|
||||
const Scalar _tmp28 = _tmp2 - 2 * _tmp21;
|
||||
const Scalar _tmp29 = -_tmp6;
|
||||
const Scalar _tmp30 = _tmp29 + _tmp8;
|
||||
const Scalar _tmp31 = _tmp18 + _tmp19;
|
||||
const Scalar _tmp32 = _tmp30 * _tmp4 + _tmp31 * state(6, 0);
|
||||
const Scalar _tmp33 = (_tmp10 * _tmp28 + _tmp32) / std::pow(_tmp16, Scalar(2));
|
||||
const Scalar _tmp34 = _tmp33 * (_tmp10 * (-_tmp18 + _tmp19) + _tmp4 * (_tmp12 + _tmp25) +
|
||||
state(6, 0) * (_tmp21 + _tmp26 + _tmp27));
|
||||
const Scalar _tmp35 =
|
||||
_tmp17 * (_tmp10 * (_tmp29 - _tmp8) + _tmp4 * (_tmp0 + _tmp1 + _tmp22 + _tmp26) +
|
||||
state(6, 0) * (_tmp11 + _tmp25)) -
|
||||
_tmp33 * (_tmp10 * (_tmp23 + _tmp27) + _tmp32);
|
||||
const Scalar _tmp36 = _tmp3 * _tmp33;
|
||||
const Scalar _tmp37 = _tmp17 * _tmp30;
|
||||
const Scalar _tmp38 = -_tmp36 + _tmp37;
|
||||
const Scalar _tmp39 = _tmp33 * _tmp9;
|
||||
const Scalar _tmp40 = _tmp17 * _tmp28;
|
||||
const Scalar _tmp41 = -_tmp39 + _tmp40;
|
||||
const Scalar _tmp42 = -_tmp14 * _tmp33 + _tmp17 * _tmp31;
|
||||
const Scalar _tmp43 = _tmp36 - _tmp37;
|
||||
const Scalar _tmp44 = _tmp39 - _tmp40;
|
||||
const Scalar _tmp45 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
|
||||
|
||||
// Output terms (2)
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp20;
|
||||
_h(1, 0) = _tmp26;
|
||||
_h(2, 0) = _tmp28;
|
||||
_h(3, 0) = _tmp29;
|
||||
_h(4, 0) = _tmp32;
|
||||
_h(5, 0) = _tmp35;
|
||||
_h(6, 0) = _tmp36;
|
||||
_h(22, 0) = _tmp37;
|
||||
_h(23, 0) = _tmp38;
|
||||
_h(0, 0) = _tmp24;
|
||||
_h(1, 0) = -_tmp34;
|
||||
_h(2, 0) = _tmp35;
|
||||
_h(3, 0) = _tmp38;
|
||||
_h(4, 0) = _tmp41;
|
||||
_h(5, 0) = _tmp42;
|
||||
_h(21, 0) = _tmp43;
|
||||
_h(22, 0) = _tmp44;
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
|
||||
matrix::Matrix<Scalar, 23, 1>& _k = (*K);
|
||||
|
||||
_k(0, 0) = _tmp39 * (P(0, 0) * _tmp20 + P(0, 1) * _tmp26 + P(0, 2) * _tmp28 +
|
||||
P(0, 22) * _tmp37 + P(0, 23) * _tmp38 + P(0, 3) * _tmp29 +
|
||||
P(0, 4) * _tmp32 + P(0, 5) * _tmp35 + P(0, 6) * _tmp36);
|
||||
_k(1, 0) = _tmp39 * (P(1, 0) * _tmp20 + P(1, 1) * _tmp26 + P(1, 2) * _tmp28 +
|
||||
P(1, 22) * _tmp37 + P(1, 23) * _tmp38 + P(1, 3) * _tmp29 +
|
||||
P(1, 4) * _tmp32 + P(1, 5) * _tmp35 + P(1, 6) * _tmp36);
|
||||
_k(2, 0) = _tmp39 * (P(2, 0) * _tmp20 + P(2, 1) * _tmp26 + P(2, 2) * _tmp28 +
|
||||
P(2, 22) * _tmp37 + P(2, 23) * _tmp38 + P(2, 3) * _tmp29 +
|
||||
P(2, 4) * _tmp32 + P(2, 5) * _tmp35 + P(2, 6) * _tmp36);
|
||||
_k(3, 0) = _tmp39 * (P(3, 0) * _tmp20 + P(3, 1) * _tmp26 + P(3, 2) * _tmp28 +
|
||||
P(3, 22) * _tmp37 + P(3, 23) * _tmp38 + P(3, 3) * _tmp29 +
|
||||
P(3, 4) * _tmp32 + P(3, 5) * _tmp35 + P(3, 6) * _tmp36);
|
||||
_k(4, 0) = _tmp39 * (P(4, 0) * _tmp20 + P(4, 1) * _tmp26 + P(4, 2) * _tmp28 +
|
||||
P(4, 22) * _tmp37 + P(4, 23) * _tmp38 + P(4, 3) * _tmp29 +
|
||||
P(4, 4) * _tmp32 + P(4, 5) * _tmp35 + P(4, 6) * _tmp36);
|
||||
_k(5, 0) = _tmp39 * (P(5, 0) * _tmp20 + P(5, 1) * _tmp26 + P(5, 2) * _tmp28 +
|
||||
P(5, 22) * _tmp37 + P(5, 23) * _tmp38 + P(5, 3) * _tmp29 +
|
||||
P(5, 4) * _tmp32 + P(5, 5) * _tmp35 + P(5, 6) * _tmp36);
|
||||
_k(6, 0) = _tmp39 * (P(6, 0) * _tmp20 + P(6, 1) * _tmp26 + P(6, 2) * _tmp28 +
|
||||
P(6, 22) * _tmp37 + P(6, 23) * _tmp38 + P(6, 3) * _tmp29 +
|
||||
P(6, 4) * _tmp32 + P(6, 5) * _tmp35 + P(6, 6) * _tmp36);
|
||||
_k(7, 0) = _tmp39 * (P(7, 0) * _tmp20 + P(7, 1) * _tmp26 + P(7, 2) * _tmp28 +
|
||||
P(7, 22) * _tmp37 + P(7, 23) * _tmp38 + P(7, 3) * _tmp29 +
|
||||
P(7, 4) * _tmp32 + P(7, 5) * _tmp35 + P(7, 6) * _tmp36);
|
||||
_k(8, 0) = _tmp39 * (P(8, 0) * _tmp20 + P(8, 1) * _tmp26 + P(8, 2) * _tmp28 +
|
||||
P(8, 22) * _tmp37 + P(8, 23) * _tmp38 + P(8, 3) * _tmp29 +
|
||||
P(8, 4) * _tmp32 + P(8, 5) * _tmp35 + P(8, 6) * _tmp36);
|
||||
_k(9, 0) = _tmp39 * (P(9, 0) * _tmp20 + P(9, 1) * _tmp26 + P(9, 2) * _tmp28 +
|
||||
P(9, 22) * _tmp37 + P(9, 23) * _tmp38 + P(9, 3) * _tmp29 +
|
||||
P(9, 4) * _tmp32 + P(9, 5) * _tmp35 + P(9, 6) * _tmp36);
|
||||
_k(10, 0) = _tmp39 * (P(10, 0) * _tmp20 + P(10, 1) * _tmp26 + P(10, 2) * _tmp28 +
|
||||
P(10, 22) * _tmp37 + P(10, 23) * _tmp38 + P(10, 3) * _tmp29 +
|
||||
P(10, 4) * _tmp32 + P(10, 5) * _tmp35 + P(10, 6) * _tmp36);
|
||||
_k(11, 0) = _tmp39 * (P(11, 0) * _tmp20 + P(11, 1) * _tmp26 + P(11, 2) * _tmp28 +
|
||||
P(11, 22) * _tmp37 + P(11, 23) * _tmp38 + P(11, 3) * _tmp29 +
|
||||
P(11, 4) * _tmp32 + P(11, 5) * _tmp35 + P(11, 6) * _tmp36);
|
||||
_k(12, 0) = _tmp39 * (P(12, 0) * _tmp20 + P(12, 1) * _tmp26 + P(12, 2) * _tmp28 +
|
||||
P(12, 22) * _tmp37 + P(12, 23) * _tmp38 + P(12, 3) * _tmp29 +
|
||||
P(12, 4) * _tmp32 + P(12, 5) * _tmp35 + P(12, 6) * _tmp36);
|
||||
_k(13, 0) = _tmp39 * (P(13, 0) * _tmp20 + P(13, 1) * _tmp26 + P(13, 2) * _tmp28 +
|
||||
P(13, 22) * _tmp37 + P(13, 23) * _tmp38 + P(13, 3) * _tmp29 +
|
||||
P(13, 4) * _tmp32 + P(13, 5) * _tmp35 + P(13, 6) * _tmp36);
|
||||
_k(14, 0) = _tmp39 * (P(14, 0) * _tmp20 + P(14, 1) * _tmp26 + P(14, 2) * _tmp28 +
|
||||
P(14, 22) * _tmp37 + P(14, 23) * _tmp38 + P(14, 3) * _tmp29 +
|
||||
P(14, 4) * _tmp32 + P(14, 5) * _tmp35 + P(14, 6) * _tmp36);
|
||||
_k(15, 0) = _tmp39 * (P(15, 0) * _tmp20 + P(15, 1) * _tmp26 + P(15, 2) * _tmp28 +
|
||||
P(15, 22) * _tmp37 + P(15, 23) * _tmp38 + P(15, 3) * _tmp29 +
|
||||
P(15, 4) * _tmp32 + P(15, 5) * _tmp35 + P(15, 6) * _tmp36);
|
||||
_k(16, 0) = _tmp39 * (P(16, 0) * _tmp20 + P(16, 1) * _tmp26 + P(16, 2) * _tmp28 +
|
||||
P(16, 22) * _tmp37 + P(16, 23) * _tmp38 + P(16, 3) * _tmp29 +
|
||||
P(16, 4) * _tmp32 + P(16, 5) * _tmp35 + P(16, 6) * _tmp36);
|
||||
_k(17, 0) = _tmp39 * (P(17, 0) * _tmp20 + P(17, 1) * _tmp26 + P(17, 2) * _tmp28 +
|
||||
P(17, 22) * _tmp37 + P(17, 23) * _tmp38 + P(17, 3) * _tmp29 +
|
||||
P(17, 4) * _tmp32 + P(17, 5) * _tmp35 + P(17, 6) * _tmp36);
|
||||
_k(18, 0) = _tmp39 * (P(18, 0) * _tmp20 + P(18, 1) * _tmp26 + P(18, 2) * _tmp28 +
|
||||
P(18, 22) * _tmp37 + P(18, 23) * _tmp38 + P(18, 3) * _tmp29 +
|
||||
P(18, 4) * _tmp32 + P(18, 5) * _tmp35 + P(18, 6) * _tmp36);
|
||||
_k(19, 0) = _tmp39 * (P(19, 0) * _tmp20 + P(19, 1) * _tmp26 + P(19, 2) * _tmp28 +
|
||||
P(19, 22) * _tmp37 + P(19, 23) * _tmp38 + P(19, 3) * _tmp29 +
|
||||
P(19, 4) * _tmp32 + P(19, 5) * _tmp35 + P(19, 6) * _tmp36);
|
||||
_k(20, 0) = _tmp39 * (P(20, 0) * _tmp20 + P(20, 1) * _tmp26 + P(20, 2) * _tmp28 +
|
||||
P(20, 22) * _tmp37 + P(20, 23) * _tmp38 + P(20, 3) * _tmp29 +
|
||||
P(20, 4) * _tmp32 + P(20, 5) * _tmp35 + P(20, 6) * _tmp36);
|
||||
_k(21, 0) = _tmp39 * (P(21, 0) * _tmp20 + P(21, 1) * _tmp26 + P(21, 2) * _tmp28 +
|
||||
P(21, 22) * _tmp37 + P(21, 23) * _tmp38 + P(21, 3) * _tmp29 +
|
||||
P(21, 4) * _tmp32 + P(21, 5) * _tmp35 + P(21, 6) * _tmp36);
|
||||
_k(22, 0) = _tmp39 * (P(22, 0) * _tmp20 + P(22, 1) * _tmp26 + P(22, 2) * _tmp28 +
|
||||
P(22, 22) * _tmp37 + P(22, 23) * _tmp38 + P(22, 3) * _tmp29 +
|
||||
P(22, 4) * _tmp32 + P(22, 5) * _tmp35 + P(22, 6) * _tmp36);
|
||||
_k(23, 0) = _tmp39 * (P(23, 0) * _tmp20 + P(23, 1) * _tmp26 + P(23, 2) * _tmp28 +
|
||||
P(23, 22) * _tmp37 + P(23, 23) * _tmp38 + P(23, 3) * _tmp29 +
|
||||
P(23, 4) * _tmp32 + P(23, 5) * _tmp35 + P(23, 6) * _tmp36);
|
||||
_k(0, 0) =
|
||||
_tmp45 * (P(0, 0) * _tmp24 - P(0, 1) * _tmp34 + P(0, 2) * _tmp35 + P(0, 21) * _tmp43 +
|
||||
P(0, 22) * _tmp44 + P(0, 3) * _tmp38 + P(0, 4) * _tmp41 + P(0, 5) * _tmp42);
|
||||
_k(1, 0) =
|
||||
_tmp45 * (P(1, 0) * _tmp24 - P(1, 1) * _tmp34 + P(1, 2) * _tmp35 + P(1, 21) * _tmp43 +
|
||||
P(1, 22) * _tmp44 + P(1, 3) * _tmp38 + P(1, 4) * _tmp41 + P(1, 5) * _tmp42);
|
||||
_k(2, 0) =
|
||||
_tmp45 * (P(2, 0) * _tmp24 - P(2, 1) * _tmp34 + P(2, 2) * _tmp35 + P(2, 21) * _tmp43 +
|
||||
P(2, 22) * _tmp44 + P(2, 3) * _tmp38 + P(2, 4) * _tmp41 + P(2, 5) * _tmp42);
|
||||
_k(3, 0) =
|
||||
_tmp45 * (P(3, 0) * _tmp24 - P(3, 1) * _tmp34 + P(3, 2) * _tmp35 + P(3, 21) * _tmp43 +
|
||||
P(3, 22) * _tmp44 + P(3, 3) * _tmp38 + P(3, 4) * _tmp41 + P(3, 5) * _tmp42);
|
||||
_k(4, 0) =
|
||||
_tmp45 * (P(4, 0) * _tmp24 - P(4, 1) * _tmp34 + P(4, 2) * _tmp35 + P(4, 21) * _tmp43 +
|
||||
P(4, 22) * _tmp44 + P(4, 3) * _tmp38 + P(4, 4) * _tmp41 + P(4, 5) * _tmp42);
|
||||
_k(5, 0) =
|
||||
_tmp45 * (P(5, 0) * _tmp24 - P(5, 1) * _tmp34 + P(5, 2) * _tmp35 + P(5, 21) * _tmp43 +
|
||||
P(5, 22) * _tmp44 + P(5, 3) * _tmp38 + P(5, 4) * _tmp41 + P(5, 5) * _tmp42);
|
||||
_k(6, 0) =
|
||||
_tmp45 * (P(6, 0) * _tmp24 - P(6, 1) * _tmp34 + P(6, 2) * _tmp35 + P(6, 21) * _tmp43 +
|
||||
P(6, 22) * _tmp44 + P(6, 3) * _tmp38 + P(6, 4) * _tmp41 + P(6, 5) * _tmp42);
|
||||
_k(7, 0) =
|
||||
_tmp45 * (P(7, 0) * _tmp24 - P(7, 1) * _tmp34 + P(7, 2) * _tmp35 + P(7, 21) * _tmp43 +
|
||||
P(7, 22) * _tmp44 + P(7, 3) * _tmp38 + P(7, 4) * _tmp41 + P(7, 5) * _tmp42);
|
||||
_k(8, 0) =
|
||||
_tmp45 * (P(8, 0) * _tmp24 - P(8, 1) * _tmp34 + P(8, 2) * _tmp35 + P(8, 21) * _tmp43 +
|
||||
P(8, 22) * _tmp44 + P(8, 3) * _tmp38 + P(8, 4) * _tmp41 + P(8, 5) * _tmp42);
|
||||
_k(9, 0) =
|
||||
_tmp45 * (P(9, 0) * _tmp24 - P(9, 1) * _tmp34 + P(9, 2) * _tmp35 + P(9, 21) * _tmp43 +
|
||||
P(9, 22) * _tmp44 + P(9, 3) * _tmp38 + P(9, 4) * _tmp41 + P(9, 5) * _tmp42);
|
||||
_k(10, 0) =
|
||||
_tmp45 * (P(10, 0) * _tmp24 - P(10, 1) * _tmp34 + P(10, 2) * _tmp35 + P(10, 21) * _tmp43 +
|
||||
P(10, 22) * _tmp44 + P(10, 3) * _tmp38 + P(10, 4) * _tmp41 + P(10, 5) * _tmp42);
|
||||
_k(11, 0) =
|
||||
_tmp45 * (P(11, 0) * _tmp24 - P(11, 1) * _tmp34 + P(11, 2) * _tmp35 + P(11, 21) * _tmp43 +
|
||||
P(11, 22) * _tmp44 + P(11, 3) * _tmp38 + P(11, 4) * _tmp41 + P(11, 5) * _tmp42);
|
||||
_k(12, 0) =
|
||||
_tmp45 * (P(12, 0) * _tmp24 - P(12, 1) * _tmp34 + P(12, 2) * _tmp35 + P(12, 21) * _tmp43 +
|
||||
P(12, 22) * _tmp44 + P(12, 3) * _tmp38 + P(12, 4) * _tmp41 + P(12, 5) * _tmp42);
|
||||
_k(13, 0) =
|
||||
_tmp45 * (P(13, 0) * _tmp24 - P(13, 1) * _tmp34 + P(13, 2) * _tmp35 + P(13, 21) * _tmp43 +
|
||||
P(13, 22) * _tmp44 + P(13, 3) * _tmp38 + P(13, 4) * _tmp41 + P(13, 5) * _tmp42);
|
||||
_k(14, 0) =
|
||||
_tmp45 * (P(14, 0) * _tmp24 - P(14, 1) * _tmp34 + P(14, 2) * _tmp35 + P(14, 21) * _tmp43 +
|
||||
P(14, 22) * _tmp44 + P(14, 3) * _tmp38 + P(14, 4) * _tmp41 + P(14, 5) * _tmp42);
|
||||
_k(15, 0) =
|
||||
_tmp45 * (P(15, 0) * _tmp24 - P(15, 1) * _tmp34 + P(15, 2) * _tmp35 + P(15, 21) * _tmp43 +
|
||||
P(15, 22) * _tmp44 + P(15, 3) * _tmp38 + P(15, 4) * _tmp41 + P(15, 5) * _tmp42);
|
||||
_k(16, 0) =
|
||||
_tmp45 * (P(16, 0) * _tmp24 - P(16, 1) * _tmp34 + P(16, 2) * _tmp35 + P(16, 21) * _tmp43 +
|
||||
P(16, 22) * _tmp44 + P(16, 3) * _tmp38 + P(16, 4) * _tmp41 + P(16, 5) * _tmp42);
|
||||
_k(17, 0) =
|
||||
_tmp45 * (P(17, 0) * _tmp24 - P(17, 1) * _tmp34 + P(17, 2) * _tmp35 + P(17, 21) * _tmp43 +
|
||||
P(17, 22) * _tmp44 + P(17, 3) * _tmp38 + P(17, 4) * _tmp41 + P(17, 5) * _tmp42);
|
||||
_k(18, 0) =
|
||||
_tmp45 * (P(18, 0) * _tmp24 - P(18, 1) * _tmp34 + P(18, 2) * _tmp35 + P(18, 21) * _tmp43 +
|
||||
P(18, 22) * _tmp44 + P(18, 3) * _tmp38 + P(18, 4) * _tmp41 + P(18, 5) * _tmp42);
|
||||
_k(19, 0) =
|
||||
_tmp45 * (P(19, 0) * _tmp24 - P(19, 1) * _tmp34 + P(19, 2) * _tmp35 + P(19, 21) * _tmp43 +
|
||||
P(19, 22) * _tmp44 + P(19, 3) * _tmp38 + P(19, 4) * _tmp41 + P(19, 5) * _tmp42);
|
||||
_k(20, 0) =
|
||||
_tmp45 * (P(20, 0) * _tmp24 - P(20, 1) * _tmp34 + P(20, 2) * _tmp35 + P(20, 21) * _tmp43 +
|
||||
P(20, 22) * _tmp44 + P(20, 3) * _tmp38 + P(20, 4) * _tmp41 + P(20, 5) * _tmp42);
|
||||
_k(21, 0) =
|
||||
_tmp45 * (P(21, 0) * _tmp24 - P(21, 1) * _tmp34 + P(21, 2) * _tmp35 + P(21, 21) * _tmp43 +
|
||||
P(21, 22) * _tmp44 + P(21, 3) * _tmp38 + P(21, 4) * _tmp41 + P(21, 5) * _tmp42);
|
||||
_k(22, 0) =
|
||||
_tmp45 * (P(22, 0) * _tmp24 - P(22, 1) * _tmp34 + P(22, 2) * _tmp35 + P(22, 21) * _tmp43 +
|
||||
P(22, 22) * _tmp44 + P(22, 3) * _tmp38 + P(22, 4) * _tmp41 + P(22, 5) * _tmp42);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+75
-76
@@ -17,7 +17,7 @@ namespace sym {
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* P: Matrix23_23
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
@@ -27,96 +27,95 @@ namespace sym {
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov = nullptr,
|
||||
Scalar* const innov_var = nullptr) {
|
||||
// Total ops: 269
|
||||
// Total ops: 235
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (39)
|
||||
const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = _tmp0 - 2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp2 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp3 = 2 * state(3, 0);
|
||||
const Scalar _tmp4 = _tmp3 * state(0, 0);
|
||||
const Scalar _tmp5 = 2 * state(1, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(2, 0);
|
||||
const Scalar _tmp7 = _tmp4 + _tmp6;
|
||||
const Scalar _tmp8 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp9 = 2 * state(2, 0);
|
||||
const Scalar _tmp10 = _tmp3 * state(1, 0) - _tmp9 * state(0, 0);
|
||||
const Scalar _tmp11 = _tmp1 * _tmp2 + _tmp10 * state(6, 0) + _tmp7 * _tmp8;
|
||||
const Scalar _tmp12 =
|
||||
_tmp11 + epsilon * (2 * math::min<Scalar>(0, (((_tmp11) > 0) - ((_tmp11) < 0))) + 1);
|
||||
const Scalar _tmp13 = Scalar(1.0) / (_tmp12);
|
||||
const Scalar _tmp14 = _tmp0 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp15 = -_tmp4 + _tmp6;
|
||||
const Scalar _tmp16 = _tmp3 * state(2, 0) + _tmp5 * state(0, 0);
|
||||
const Scalar _tmp17 = _tmp14 * _tmp8 + _tmp15 * _tmp2 + _tmp16 * state(6, 0);
|
||||
const Scalar _tmp18 = _tmp17 / std::pow(_tmp12, Scalar(2));
|
||||
const Scalar _tmp19 = _tmp1 * _tmp18;
|
||||
const Scalar _tmp20 = _tmp13 * _tmp15;
|
||||
const Scalar _tmp21 = -_tmp19 + _tmp20;
|
||||
const Scalar _tmp22 = _tmp3 * state(6, 0);
|
||||
const Scalar _tmp23 = 4 * _tmp8;
|
||||
const Scalar _tmp24 = 2 * state(0, 0);
|
||||
const Scalar _tmp25 = _tmp24 * state(6, 0);
|
||||
const Scalar _tmp26 =
|
||||
_tmp13 * (_tmp2 * _tmp9 - _tmp23 * state(1, 0) + _tmp25) - _tmp18 * (_tmp22 + _tmp8 * _tmp9);
|
||||
const Scalar _tmp27 = -_tmp10 * _tmp18 + _tmp13 * _tmp16;
|
||||
const Scalar _tmp28 = 4 * _tmp2;
|
||||
const Scalar _tmp29 =
|
||||
_tmp13 * (_tmp2 * _tmp5 + _tmp22) - _tmp18 * (-_tmp25 - _tmp28 * state(2, 0) + _tmp5 * _tmp8);
|
||||
const Scalar _tmp30 = _tmp9 * state(6, 0);
|
||||
const Scalar _tmp31 = _tmp5 * state(6, 0);
|
||||
const Scalar _tmp32 = _tmp13 * (-_tmp2 * _tmp3 + _tmp31) - _tmp18 * (_tmp3 * _tmp8 - _tmp30);
|
||||
const Scalar _tmp33 = _tmp19 - _tmp20;
|
||||
const Scalar _tmp34 = _tmp18 * _tmp7;
|
||||
const Scalar _tmp35 = _tmp13 * _tmp14;
|
||||
const Scalar _tmp36 = -_tmp34 + _tmp35;
|
||||
const Scalar _tmp37 = _tmp34 - _tmp35;
|
||||
const Scalar _tmp38 = _tmp13 * (-_tmp2 * _tmp24 - _tmp23 * state(3, 0) + _tmp30) -
|
||||
_tmp18 * (_tmp24 * _tmp8 - _tmp28 * state(3, 0) + _tmp31);
|
||||
// Intermediate terms (46)
|
||||
const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp2 = 1 - 2 * _tmp1;
|
||||
const Scalar _tmp3 = -2 * _tmp0 + _tmp2;
|
||||
const Scalar _tmp4 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp5 = 2 * state(0, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp7 = 2 * state(1, 0);
|
||||
const Scalar _tmp8 = _tmp7 * state(2, 0);
|
||||
const Scalar _tmp9 = _tmp6 + _tmp8;
|
||||
const Scalar _tmp10 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp11 = _tmp5 * state(2, 0);
|
||||
const Scalar _tmp12 = -_tmp11;
|
||||
const Scalar _tmp13 = _tmp7 * state(3, 0);
|
||||
const Scalar _tmp14 = _tmp12 + _tmp13;
|
||||
const Scalar _tmp15 = _tmp10 * _tmp9 + _tmp14 * state(6, 0) + _tmp3 * _tmp4;
|
||||
const Scalar _tmp16 =
|
||||
_tmp15 + epsilon * (2 * math::min<Scalar>(0, (((_tmp15) > 0) - ((_tmp15) < 0))) + 1);
|
||||
const Scalar _tmp17 = Scalar(1.0) / (_tmp16);
|
||||
const Scalar _tmp18 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp19 = -2 * _tmp18 + _tmp2;
|
||||
const Scalar _tmp20 = -_tmp6;
|
||||
const Scalar _tmp21 = _tmp20 + _tmp8;
|
||||
const Scalar _tmp22 = 2 * state(2, 0) * state(3, 0);
|
||||
const Scalar _tmp23 = _tmp5 * state(1, 0);
|
||||
const Scalar _tmp24 = _tmp22 + _tmp23;
|
||||
const Scalar _tmp25 = _tmp21 * _tmp4 + _tmp24 * state(6, 0);
|
||||
const Scalar _tmp26 = _tmp10 * _tmp19 + _tmp25;
|
||||
const Scalar _tmp27 = _tmp26 / std::pow(_tmp16, Scalar(2));
|
||||
const Scalar _tmp28 = -_tmp14 * _tmp27 + _tmp17 * _tmp24;
|
||||
const Scalar _tmp29 = _tmp27 * _tmp3;
|
||||
const Scalar _tmp30 = _tmp17 * _tmp21;
|
||||
const Scalar _tmp31 = -_tmp29 + _tmp30;
|
||||
const Scalar _tmp32 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp33 = -_tmp32;
|
||||
const Scalar _tmp34 = -_tmp18;
|
||||
const Scalar _tmp35 = -_tmp13;
|
||||
const Scalar _tmp36 = _tmp0 - _tmp1;
|
||||
const Scalar _tmp37 = _tmp32 + _tmp34;
|
||||
const Scalar _tmp38 =
|
||||
_tmp17 * (_tmp10 * (_tmp20 - _tmp8) + _tmp4 * (_tmp0 + _tmp1 + _tmp33 + _tmp34) +
|
||||
state(6, 0) * (_tmp11 + _tmp35)) -
|
||||
_tmp27 * (_tmp10 * (_tmp36 + _tmp37) + _tmp25);
|
||||
const Scalar _tmp39 = _tmp29 - _tmp30;
|
||||
const Scalar _tmp40 = _tmp27 * _tmp9;
|
||||
const Scalar _tmp41 = _tmp17 * _tmp19;
|
||||
const Scalar _tmp42 = -_tmp40 + _tmp41;
|
||||
const Scalar _tmp43 = _tmp27 * (_tmp10 * (-_tmp22 + _tmp23) + _tmp4 * (_tmp12 + _tmp35) +
|
||||
state(6, 0) * (_tmp18 + _tmp33 + _tmp36));
|
||||
const Scalar _tmp44 = _tmp40 - _tmp41;
|
||||
const Scalar _tmp45 = _tmp17 * (_tmp10 * (_tmp22 - _tmp23) + _tmp4 * (_tmp11 + _tmp13) +
|
||||
state(6, 0) * (-_tmp0 + _tmp1 + _tmp37));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov != nullptr) {
|
||||
Scalar& _innov = (*innov);
|
||||
|
||||
_innov = _tmp13 * _tmp17;
|
||||
_innov = _tmp17 * _tmp26;
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R +
|
||||
_tmp21 * (P(0, 4) * _tmp32 + P(1, 4) * _tmp26 + P(2, 4) * _tmp29 +
|
||||
P(22, 4) * _tmp33 + P(23, 4) * _tmp37 + P(3, 4) * _tmp38 +
|
||||
P(4, 4) * _tmp21 + P(5, 4) * _tmp36 + P(6, 4) * _tmp27) +
|
||||
_tmp26 * (P(0, 1) * _tmp32 + P(1, 1) * _tmp26 + P(2, 1) * _tmp29 +
|
||||
P(22, 1) * _tmp33 + P(23, 1) * _tmp37 + P(3, 1) * _tmp38 +
|
||||
P(4, 1) * _tmp21 + P(5, 1) * _tmp36 + P(6, 1) * _tmp27) +
|
||||
_tmp27 * (P(0, 6) * _tmp32 + P(1, 6) * _tmp26 + P(2, 6) * _tmp29 +
|
||||
P(22, 6) * _tmp33 + P(23, 6) * _tmp37 + P(3, 6) * _tmp38 +
|
||||
P(4, 6) * _tmp21 + P(5, 6) * _tmp36 + P(6, 6) * _tmp27) +
|
||||
_tmp29 * (P(0, 2) * _tmp32 + P(1, 2) * _tmp26 + P(2, 2) * _tmp29 +
|
||||
P(22, 2) * _tmp33 + P(23, 2) * _tmp37 + P(3, 2) * _tmp38 +
|
||||
P(4, 2) * _tmp21 + P(5, 2) * _tmp36 + P(6, 2) * _tmp27) +
|
||||
_tmp32 * (P(0, 0) * _tmp32 + P(1, 0) * _tmp26 + P(2, 0) * _tmp29 +
|
||||
P(22, 0) * _tmp33 + P(23, 0) * _tmp37 + P(3, 0) * _tmp38 +
|
||||
P(4, 0) * _tmp21 + P(5, 0) * _tmp36 + P(6, 0) * _tmp27) +
|
||||
_tmp33 * (P(0, 22) * _tmp32 + P(1, 22) * _tmp26 + P(2, 22) * _tmp29 +
|
||||
P(22, 22) * _tmp33 + P(23, 22) * _tmp37 + P(3, 22) * _tmp38 +
|
||||
P(4, 22) * _tmp21 + P(5, 22) * _tmp36 + P(6, 22) * _tmp27) +
|
||||
_tmp36 * (P(0, 5) * _tmp32 + P(1, 5) * _tmp26 + P(2, 5) * _tmp29 +
|
||||
P(22, 5) * _tmp33 + P(23, 5) * _tmp37 + P(3, 5) * _tmp38 +
|
||||
P(4, 5) * _tmp21 + P(5, 5) * _tmp36 + P(6, 5) * _tmp27) +
|
||||
_tmp37 * (P(0, 23) * _tmp32 + P(1, 23) * _tmp26 + P(2, 23) * _tmp29 +
|
||||
P(22, 23) * _tmp33 + P(23, 23) * _tmp37 + P(3, 23) * _tmp38 +
|
||||
P(4, 23) * _tmp21 + P(5, 23) * _tmp36 + P(6, 23) * _tmp27) +
|
||||
_tmp38 * (P(0, 3) * _tmp32 + P(1, 3) * _tmp26 + P(2, 3) * _tmp29 +
|
||||
P(22, 3) * _tmp33 + P(23, 3) * _tmp37 + P(3, 3) * _tmp38 +
|
||||
P(4, 3) * _tmp21 + P(5, 3) * _tmp36 + P(6, 3) * _tmp27);
|
||||
_innov_var =
|
||||
R +
|
||||
_tmp28 * (P(0, 5) * _tmp45 - P(1, 5) * _tmp43 + P(2, 5) * _tmp38 + P(21, 5) * _tmp39 +
|
||||
P(22, 5) * _tmp44 + P(3, 5) * _tmp31 + P(4, 5) * _tmp42 + P(5, 5) * _tmp28) +
|
||||
_tmp31 * (P(0, 3) * _tmp45 - P(1, 3) * _tmp43 + P(2, 3) * _tmp38 + P(21, 3) * _tmp39 +
|
||||
P(22, 3) * _tmp44 + P(3, 3) * _tmp31 + P(4, 3) * _tmp42 + P(5, 3) * _tmp28) +
|
||||
_tmp38 * (P(0, 2) * _tmp45 - P(1, 2) * _tmp43 + P(2, 2) * _tmp38 + P(21, 2) * _tmp39 +
|
||||
P(22, 2) * _tmp44 + P(3, 2) * _tmp31 + P(4, 2) * _tmp42 + P(5, 2) * _tmp28) +
|
||||
_tmp39 * (P(0, 21) * _tmp45 - P(1, 21) * _tmp43 + P(2, 21) * _tmp38 + P(21, 21) * _tmp39 +
|
||||
P(22, 21) * _tmp44 + P(3, 21) * _tmp31 + P(4, 21) * _tmp42 + P(5, 21) * _tmp28) +
|
||||
_tmp42 * (P(0, 4) * _tmp45 - P(1, 4) * _tmp43 + P(2, 4) * _tmp38 + P(21, 4) * _tmp39 +
|
||||
P(22, 4) * _tmp44 + P(3, 4) * _tmp31 + P(4, 4) * _tmp42 + P(5, 4) * _tmp28) -
|
||||
_tmp43 * (P(0, 1) * _tmp45 - P(1, 1) * _tmp43 + P(2, 1) * _tmp38 + P(21, 1) * _tmp39 +
|
||||
P(22, 1) * _tmp44 + P(3, 1) * _tmp31 + P(4, 1) * _tmp42 + P(5, 1) * _tmp28) +
|
||||
_tmp44 * (P(0, 22) * _tmp45 - P(1, 22) * _tmp43 + P(2, 22) * _tmp38 + P(21, 22) * _tmp39 +
|
||||
P(22, 22) * _tmp44 + P(3, 22) * _tmp31 + P(4, 22) * _tmp42 + P(5, 22) * _tmp28) +
|
||||
_tmp45 * (P(0, 0) * _tmp45 - P(1, 0) * _tmp43 + P(2, 0) * _tmp38 + P(21, 0) * _tmp39 +
|
||||
P(22, 0) * _tmp44 + P(3, 0) * _tmp31 + P(4, 0) * _tmp42 + P(5, 0) * _tmp28);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+27
-30
@@ -17,61 +17,58 @@ namespace sym {
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* P: Matrix23_23
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
* H: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeYaw312InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 73
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
// Total ops: 53
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (15)
|
||||
const Scalar _tmp0 = 2 * state(0, 0);
|
||||
const Scalar _tmp1 = 2 * state(2, 0);
|
||||
const Scalar _tmp2 = -_tmp0 * state(3, 0) + _tmp1 * state(1, 0);
|
||||
const Scalar _tmp3 =
|
||||
-2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
|
||||
const Scalar _tmp4 = _tmp3 + epsilon * ((((_tmp3) > 0) - ((_tmp3) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp5 = std::pow(_tmp4, Scalar(2));
|
||||
const Scalar _tmp6 = 4 * _tmp2 / _tmp5;
|
||||
const Scalar _tmp7 = Scalar(1.0) / (_tmp4);
|
||||
const Scalar _tmp8 = Scalar(1.0) / (std::pow(_tmp2, Scalar(2)) + _tmp5);
|
||||
const Scalar _tmp9 = _tmp5 * _tmp8;
|
||||
const Scalar _tmp10 = _tmp9 * (-_tmp1 * _tmp7 - _tmp6 * state(1, 0));
|
||||
const Scalar _tmp11 = _tmp9 * (_tmp0 * _tmp7 - _tmp6 * state(3, 0));
|
||||
const Scalar _tmp12 = 2 * _tmp4 * _tmp8;
|
||||
const Scalar _tmp13 = _tmp12 * state(3, 0);
|
||||
const Scalar _tmp14 = _tmp12 * state(1, 0);
|
||||
const Scalar _tmp1 = -_tmp0 * state(3, 0);
|
||||
const Scalar _tmp2 = 2 * state(1, 0);
|
||||
const Scalar _tmp3 = _tmp2 * state(2, 0);
|
||||
const Scalar _tmp4 = _tmp1 + _tmp3;
|
||||
const Scalar _tmp5 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp6 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp7 = -2 * _tmp5 - 2 * _tmp6 + 1;
|
||||
const Scalar _tmp8 = _tmp7 + epsilon * ((((_tmp7) > 0) - ((_tmp7) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp9 = std::pow(_tmp8, Scalar(2));
|
||||
const Scalar _tmp10 = _tmp4 / _tmp9;
|
||||
const Scalar _tmp11 = Scalar(1.0) / (_tmp8);
|
||||
const Scalar _tmp12 = _tmp9 / (std::pow(_tmp4, Scalar(2)) + _tmp9);
|
||||
const Scalar _tmp13 = _tmp12 * (_tmp10 * (_tmp1 - _tmp3) -
|
||||
_tmp11 * (_tmp5 - _tmp6 - std::pow(state(0, 0), Scalar(2)) +
|
||||
std::pow(state(2, 0), Scalar(2))));
|
||||
const Scalar _tmp14 = _tmp12 * (_tmp10 * (-_tmp2 * state(0, 0) + 2 * state(2, 0) * state(3, 0)) -
|
||||
_tmp11 * (_tmp0 * state(2, 0) + _tmp2 * state(3, 0)));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var =
|
||||
R + _tmp10 * (P(0, 1) * _tmp13 + P(1, 1) * _tmp10 - P(2, 1) * _tmp14 + P(3, 1) * _tmp11) +
|
||||
_tmp11 * (P(0, 3) * _tmp13 + P(1, 3) * _tmp10 - P(2, 3) * _tmp14 + P(3, 3) * _tmp11) +
|
||||
_tmp13 * (P(0, 0) * _tmp13 + P(1, 0) * _tmp10 - P(2, 0) * _tmp14 + P(3, 0) * _tmp11) -
|
||||
_tmp14 * (P(0, 2) * _tmp13 + P(1, 2) * _tmp10 - P(2, 2) * _tmp14 + P(3, 2) * _tmp11);
|
||||
_innov_var = R + _tmp13 * (P(0, 2) * _tmp14 + P(2, 2) * _tmp13) +
|
||||
_tmp14 * (P(0, 0) * _tmp14 + P(2, 0) * _tmp13);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp13;
|
||||
_h(1, 0) = _tmp10;
|
||||
_h(2, 0) = -_tmp14;
|
||||
_h(3, 0) = _tmp11;
|
||||
_h(0, 0) = _tmp14;
|
||||
_h(2, 0) = _tmp13;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+27
-30
@@ -17,61 +17,58 @@ namespace sym {
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* P: Matrix23_23
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
* H: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeYaw312InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 73
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
// Total ops: 57
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (15)
|
||||
const Scalar _tmp0 = 2 * state(0, 0);
|
||||
const Scalar _tmp1 =
|
||||
-2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
|
||||
const Scalar _tmp2 = 2 * state(2, 0);
|
||||
const Scalar _tmp3 = -_tmp0 * state(3, 0) + _tmp2 * state(1, 0);
|
||||
const Scalar _tmp4 = _tmp3 + epsilon * ((((_tmp3) > 0) - ((_tmp3) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp5 = std::pow(_tmp4, Scalar(2));
|
||||
const Scalar _tmp6 = _tmp1 / _tmp5;
|
||||
const Scalar _tmp7 = 4 / _tmp4;
|
||||
const Scalar _tmp8 = Scalar(1.0) / (std::pow(_tmp1, Scalar(2)) + _tmp5);
|
||||
const Scalar _tmp9 = _tmp5 * _tmp8;
|
||||
const Scalar _tmp10 = _tmp9 * (-_tmp0 * _tmp6 + _tmp7 * state(3, 0));
|
||||
const Scalar _tmp11 = 2 * _tmp1 * _tmp8;
|
||||
const Scalar _tmp12 = _tmp11 * state(3, 0);
|
||||
const Scalar _tmp13 = _tmp11 * state(1, 0);
|
||||
const Scalar _tmp14 = _tmp9 * (_tmp2 * _tmp6 + _tmp7 * state(1, 0));
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp3 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp4 = -2 * _tmp2 - 2 * _tmp3 + 1;
|
||||
const Scalar _tmp5 = -_tmp0 * state(3, 0);
|
||||
const Scalar _tmp6 = _tmp1 * state(2, 0);
|
||||
const Scalar _tmp7 = _tmp5 + _tmp6;
|
||||
const Scalar _tmp8 = _tmp7 + epsilon * ((((_tmp7) > 0) - ((_tmp7) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp9 = std::pow(_tmp8, Scalar(2));
|
||||
const Scalar _tmp10 = _tmp4 / _tmp9;
|
||||
const Scalar _tmp11 = Scalar(1.0) / (_tmp8);
|
||||
const Scalar _tmp12 = _tmp9 / (std::pow(_tmp4, Scalar(2)) + _tmp9);
|
||||
const Scalar _tmp13 = _tmp12 * (_tmp10 * (_tmp0 * state(2, 0) + _tmp1 * state(3, 0)) -
|
||||
_tmp11 * (-_tmp1 * state(0, 0) + 2 * state(2, 0) * state(3, 0)));
|
||||
const Scalar _tmp14 = _tmp12 * (_tmp10 * (_tmp2 - _tmp3 - std::pow(state(0, 0), Scalar(2)) +
|
||||
std::pow(state(2, 0), Scalar(2))) -
|
||||
_tmp11 * (_tmp5 - _tmp6));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var =
|
||||
R - _tmp10 * (P(0, 3) * _tmp12 - P(1, 3) * _tmp14 - P(2, 3) * _tmp13 - P(3, 3) * _tmp10) +
|
||||
_tmp12 * (P(0, 0) * _tmp12 - P(1, 0) * _tmp14 - P(2, 0) * _tmp13 - P(3, 0) * _tmp10) -
|
||||
_tmp13 * (P(0, 2) * _tmp12 - P(1, 2) * _tmp14 - P(2, 2) * _tmp13 - P(3, 2) * _tmp10) -
|
||||
_tmp14 * (P(0, 1) * _tmp12 - P(1, 1) * _tmp14 - P(2, 1) * _tmp13 - P(3, 1) * _tmp10);
|
||||
_innov_var = R - _tmp13 * (-P(0, 0) * _tmp13 - P(2, 0) * _tmp14) -
|
||||
_tmp14 * (-P(0, 2) * _tmp13 - P(2, 2) * _tmp14);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp12;
|
||||
_h(1, 0) = -_tmp14;
|
||||
_h(2, 0) = -_tmp13;
|
||||
_h(3, 0) = -_tmp10;
|
||||
_h(0, 0) = -_tmp13;
|
||||
_h(2, 0) = -_tmp14;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+28
-31
@@ -17,61 +17,58 @@ namespace sym {
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* P: Matrix23_23
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
* H: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeYaw321InnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 70
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
// Total ops: 53
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (15)
|
||||
const Scalar _tmp0 = 2 * state(3, 0);
|
||||
const Scalar _tmp1 = 2 * state(2, 0);
|
||||
const Scalar _tmp2 = _tmp0 * state(0, 0) + _tmp1 * state(1, 0);
|
||||
const Scalar _tmp3 =
|
||||
-2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
|
||||
const Scalar _tmp4 = _tmp3 + epsilon * ((((_tmp3) > 0) - ((_tmp3) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp5 = std::pow(_tmp4, Scalar(2));
|
||||
const Scalar _tmp6 = Scalar(1.0) / (std::pow(_tmp2, Scalar(2)) + _tmp5);
|
||||
const Scalar _tmp7 = _tmp4 * _tmp6;
|
||||
const Scalar _tmp8 = _tmp0 * _tmp7;
|
||||
const Scalar _tmp9 = _tmp1 * _tmp7;
|
||||
const Scalar _tmp10 = 4 * _tmp2 / _tmp5;
|
||||
const Scalar _tmp11 = 2 / _tmp4;
|
||||
const Scalar _tmp12 = _tmp5 * _tmp6;
|
||||
const Scalar _tmp13 = _tmp12 * (_tmp10 * state(3, 0) + _tmp11 * state(0, 0));
|
||||
const Scalar _tmp14 = _tmp12 * (_tmp10 * state(2, 0) + _tmp11 * state(1, 0));
|
||||
const Scalar _tmp0 = 2 * state(0, 0);
|
||||
const Scalar _tmp1 = _tmp0 * state(3, 0);
|
||||
const Scalar _tmp2 = 2 * state(1, 0);
|
||||
const Scalar _tmp3 = _tmp2 * state(2, 0);
|
||||
const Scalar _tmp4 = _tmp1 + _tmp3;
|
||||
const Scalar _tmp5 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp6 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp7 = -2 * _tmp5 - 2 * _tmp6 + 1;
|
||||
const Scalar _tmp8 = _tmp7 + epsilon * ((((_tmp7) > 0) - ((_tmp7) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp9 = std::pow(_tmp8, Scalar(2));
|
||||
const Scalar _tmp10 = _tmp4 / _tmp9;
|
||||
const Scalar _tmp11 = Scalar(1.0) / (_tmp8);
|
||||
const Scalar _tmp12 = _tmp9 / (std::pow(_tmp4, Scalar(2)) + _tmp9);
|
||||
const Scalar _tmp13 = _tmp12 * (-_tmp10 * (-_tmp1 + _tmp3) +
|
||||
_tmp11 * (-_tmp5 + _tmp6 + std::pow(state(0, 0), Scalar(2)) -
|
||||
std::pow(state(1, 0), Scalar(2))));
|
||||
const Scalar _tmp14 = _tmp12 * (-_tmp10 * (-_tmp0 * state(2, 0) - _tmp2 * state(3, 0)) +
|
||||
_tmp11 * (_tmp2 * state(0, 0) - 2 * state(2, 0) * state(3, 0)));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var =
|
||||
R + _tmp13 * (P(0, 3) * _tmp8 + P(1, 3) * _tmp9 + P(2, 3) * _tmp14 + P(3, 3) * _tmp13) +
|
||||
_tmp14 * (P(0, 2) * _tmp8 + P(1, 2) * _tmp9 + P(2, 2) * _tmp14 + P(3, 2) * _tmp13) +
|
||||
_tmp8 * (P(0, 0) * _tmp8 + P(1, 0) * _tmp9 + P(2, 0) * _tmp14 + P(3, 0) * _tmp13) +
|
||||
_tmp9 * (P(0, 1) * _tmp8 + P(1, 1) * _tmp9 + P(2, 1) * _tmp14 + P(3, 1) * _tmp13);
|
||||
_innov_var = R + _tmp13 * (P(1, 2) * _tmp14 + P(2, 2) * _tmp13) +
|
||||
_tmp14 * (P(1, 1) * _tmp14 + P(2, 1) * _tmp13);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp8;
|
||||
_h(1, 0) = _tmp9;
|
||||
_h(2, 0) = _tmp14;
|
||||
_h(3, 0) = _tmp13;
|
||||
_h(1, 0) = _tmp14;
|
||||
_h(2, 0) = _tmp13;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+26
-29
@@ -17,61 +17,58 @@ namespace sym {
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* P: Matrix23_23
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* H: Matrix24_1
|
||||
* H: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeYaw321InnovVarAndHAlternate(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
|
||||
// Total ops: 75
|
||||
matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
|
||||
// Total ops: 57
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (15)
|
||||
const Scalar _tmp0 = 2 * state(2, 0);
|
||||
const Scalar _tmp1 =
|
||||
-2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
|
||||
const Scalar _tmp2 = 2 * state(0, 0);
|
||||
const Scalar _tmp3 = _tmp0 * state(1, 0) + _tmp2 * state(3, 0);
|
||||
const Scalar _tmp4 = _tmp3 + epsilon * ((((_tmp3) > 0) - ((_tmp3) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp5 = std::pow(_tmp4, Scalar(2));
|
||||
const Scalar _tmp6 = Scalar(1.0) / (std::pow(_tmp1, Scalar(2)) + _tmp5);
|
||||
const Scalar _tmp7 = _tmp1 * _tmp6;
|
||||
const Scalar _tmp8 = _tmp0 * _tmp7;
|
||||
const Scalar _tmp9 = 2 * _tmp7 * state(3, 0);
|
||||
const Scalar _tmp10 = _tmp1 / _tmp5;
|
||||
const Scalar _tmp11 = 4 / _tmp4;
|
||||
const Scalar _tmp12 = _tmp5 * _tmp6;
|
||||
const Scalar _tmp13 = _tmp12 * (-_tmp10 * _tmp2 - _tmp11 * state(3, 0));
|
||||
const Scalar _tmp14 = _tmp12 * (-2 * _tmp10 * state(1, 0) - _tmp11 * state(2, 0));
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 = 2 * state(0, 0) * state(3, 0);
|
||||
const Scalar _tmp3 = _tmp1 * state(2, 0);
|
||||
const Scalar _tmp4 = _tmp2 + _tmp3;
|
||||
const Scalar _tmp5 = _tmp4 + epsilon * ((((_tmp4) > 0) - ((_tmp4) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp6 = Scalar(1.0) / (_tmp5);
|
||||
const Scalar _tmp7 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp8 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp9 = -2 * _tmp7 - 2 * _tmp8 + 1;
|
||||
const Scalar _tmp10 = std::pow(_tmp5, Scalar(2));
|
||||
const Scalar _tmp11 = _tmp9 / _tmp10;
|
||||
const Scalar _tmp12 = _tmp10 / (_tmp10 + std::pow(_tmp9, Scalar(2)));
|
||||
const Scalar _tmp13 = _tmp12 * (-_tmp11 * (-_tmp0 * state(3, 0) + _tmp1 * state(0, 0)) +
|
||||
_tmp6 * (-_tmp0 * state(0, 0) - _tmp1 * state(3, 0)));
|
||||
const Scalar _tmp14 = _tmp12 * (-_tmp11 * (-_tmp7 + _tmp8 + std::pow(state(0, 0), Scalar(2)) -
|
||||
std::pow(state(1, 0), Scalar(2))) +
|
||||
_tmp6 * (-_tmp2 + _tmp3));
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var =
|
||||
R - _tmp13 * (P(0, 3) * _tmp9 + P(1, 3) * _tmp8 - P(2, 3) * _tmp14 - P(3, 3) * _tmp13) -
|
||||
_tmp14 * (P(0, 2) * _tmp9 + P(1, 2) * _tmp8 - P(2, 2) * _tmp14 - P(3, 2) * _tmp13) +
|
||||
_tmp8 * (P(0, 1) * _tmp9 + P(1, 1) * _tmp8 - P(2, 1) * _tmp14 - P(3, 1) * _tmp13) +
|
||||
_tmp9 * (P(0, 0) * _tmp9 + P(1, 0) * _tmp8 - P(2, 0) * _tmp14 - P(3, 0) * _tmp13);
|
||||
_innov_var = R - _tmp13 * (-P(1, 1) * _tmp13 - P(2, 1) * _tmp14) -
|
||||
_tmp14 * (-P(1, 2) * _tmp13 - P(2, 2) * _tmp14);
|
||||
}
|
||||
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _h = (*H);
|
||||
matrix::Matrix<Scalar, 23, 1>& _h = (*H);
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp9;
|
||||
_h(1, 0) = _tmp8;
|
||||
_h(1, 0) = -_tmp13;
|
||||
_h(2, 0) = -_tmp14;
|
||||
_h(3, 0) = -_tmp13;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,67 +0,0 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// 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: quat_var_to_rot_var
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* rot_var: Matrix31
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void QuatVarToRotVar(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar epsilon,
|
||||
matrix::Matrix<Scalar, 3, 1>* const rot_var = nullptr) {
|
||||
// Total ops: 61
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (17)
|
||||
const Scalar _tmp0 = std::fabs(state(0, 0));
|
||||
const Scalar _tmp1 = 1 - epsilon;
|
||||
const Scalar _tmp2 = math::min<Scalar>(_tmp0, _tmp1);
|
||||
const Scalar _tmp3 = 1 - std::pow(_tmp2, Scalar(2));
|
||||
const Scalar _tmp4 = (((state(0, 0)) > 0) - ((state(0, 0)) < 0));
|
||||
const Scalar _tmp5 = 2 * math::min<Scalar>(0, _tmp4) + 1;
|
||||
const Scalar _tmp6 = _tmp5 * std::acos(_tmp2);
|
||||
const Scalar _tmp7 = 2 * _tmp6 / std::sqrt(_tmp3);
|
||||
const Scalar _tmp8 = _tmp4 * ((((-_tmp0 + _tmp1) > 0) - ((-_tmp0 + _tmp1) < 0)) + 1);
|
||||
const Scalar _tmp9 = _tmp8 * state(1, 0);
|
||||
const Scalar _tmp10 = _tmp2 * _tmp6 / (_tmp3 * std::sqrt(_tmp3));
|
||||
const Scalar _tmp11 = _tmp5 / _tmp3;
|
||||
const Scalar _tmp12 = _tmp10 * _tmp9 - _tmp11 * _tmp9;
|
||||
const Scalar _tmp13 = _tmp10 * _tmp8;
|
||||
const Scalar _tmp14 = _tmp11 * _tmp8;
|
||||
const Scalar _tmp15 = _tmp13 * state(2, 0) - _tmp14 * state(2, 0);
|
||||
const Scalar _tmp16 = _tmp13 * state(3, 0) - _tmp14 * state(3, 0);
|
||||
|
||||
// Output terms (1)
|
||||
if (rot_var != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _rot_var = (*rot_var);
|
||||
|
||||
_rot_var(0, 0) = _tmp12 * (P(0, 0) * _tmp12 + P(1, 0) * _tmp7) +
|
||||
_tmp7 * (P(0, 1) * _tmp12 + P(1, 1) * _tmp7);
|
||||
_rot_var(1, 0) = _tmp15 * (P(0, 0) * _tmp15 + P(2, 0) * _tmp7) +
|
||||
_tmp7 * (P(0, 2) * _tmp15 + P(2, 2) * _tmp7);
|
||||
_rot_var(2, 0) = _tmp16 * (P(0, 0) * _tmp16 + P(3, 0) * _tmp7) +
|
||||
_tmp7 * (P(0, 3) * _tmp16 + P(3, 3) * _tmp7);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
-123
@@ -1,123 +0,0 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// 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: rot_var_ned_to_lower_triangular_quat_cov
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* rot_var_ned: Matrix31
|
||||
*
|
||||
* Outputs:
|
||||
* q_cov_lower_triangle: Matrix44
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void RotVarNedToLowerTriangularQuatCov(
|
||||
const matrix::Matrix<Scalar, 24, 1>& state, const matrix::Matrix<Scalar, 3, 1>& rot_var_ned,
|
||||
matrix::Matrix<Scalar, 4, 4>* const q_cov_lower_triangle = nullptr) {
|
||||
// Total ops: 185
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (54)
|
||||
const Scalar _tmp0 = 2 * state(0, 0) * state(2, 0);
|
||||
const Scalar _tmp1 = 2 * state(3, 0);
|
||||
const Scalar _tmp2 = _tmp1 * state(1, 0);
|
||||
const Scalar _tmp3 = _tmp0 + _tmp2;
|
||||
const Scalar _tmp4 = _tmp1 * state(0, 0);
|
||||
const Scalar _tmp5 = 2 * state(1, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(2, 0);
|
||||
const Scalar _tmp7 = -_tmp4 + _tmp6;
|
||||
const Scalar _tmp8 = -2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp9 = -2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp10 = _tmp8 + _tmp9 + 1;
|
||||
const Scalar _tmp11 = _tmp1 * state(2, 0);
|
||||
const Scalar _tmp12 = _tmp5 * state(0, 0);
|
||||
const Scalar _tmp13 = _tmp11 - _tmp12;
|
||||
const Scalar _tmp14 = _tmp13 * rot_var_ned(1, 0);
|
||||
const Scalar _tmp15 = 1 - 2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp16 = _tmp15 + _tmp9;
|
||||
const Scalar _tmp17 = _tmp11 + _tmp12;
|
||||
const Scalar _tmp18 = _tmp17 * rot_var_ned(2, 0);
|
||||
const Scalar _tmp19 = _tmp10 * _tmp14 + _tmp16 * _tmp18 + _tmp3 * _tmp7 * rot_var_ned(0, 0);
|
||||
const Scalar _tmp20 = (Scalar(1) / Scalar(2)) * state(3, 0);
|
||||
const Scalar _tmp21 = -_tmp19 * _tmp20;
|
||||
const Scalar _tmp22 = std::pow(_tmp10, Scalar(2)) * rot_var_ned(1, 0) +
|
||||
std::pow(_tmp17, Scalar(2)) * rot_var_ned(2, 0) +
|
||||
std::pow(_tmp7, Scalar(2)) * rot_var_ned(0, 0);
|
||||
const Scalar _tmp23 = (Scalar(1) / Scalar(2)) * state(2, 0);
|
||||
const Scalar _tmp24 = _tmp15 + _tmp8;
|
||||
const Scalar _tmp25 = _tmp24 * rot_var_ned(0, 0);
|
||||
const Scalar _tmp26 = _tmp4 + _tmp6;
|
||||
const Scalar _tmp27 = -_tmp0 + _tmp2;
|
||||
const Scalar _tmp28 = (Scalar(1) / Scalar(2)) * _tmp10 * _tmp26 * rot_var_ned(1, 0) +
|
||||
(Scalar(1) / Scalar(2)) * _tmp18 * _tmp27 +
|
||||
(Scalar(1) / Scalar(2)) * _tmp25 * _tmp7;
|
||||
const Scalar _tmp29 = _tmp28 * state(1, 0);
|
||||
const Scalar _tmp30 = std::pow(_tmp24, Scalar(2)) * rot_var_ned(0, 0) +
|
||||
std::pow(_tmp26, Scalar(2)) * rot_var_ned(1, 0) +
|
||||
std::pow(_tmp27, Scalar(2)) * rot_var_ned(2, 0);
|
||||
const Scalar _tmp31 = (Scalar(1) / Scalar(2)) * state(1, 0);
|
||||
const Scalar _tmp32 = _tmp14 * _tmp26 + _tmp16 * _tmp27 * rot_var_ned(2, 0) + _tmp25 * _tmp3;
|
||||
const Scalar _tmp33 = _tmp20 * _tmp32;
|
||||
const Scalar _tmp34 = -_tmp28 * state(2, 0);
|
||||
const Scalar _tmp35 = _tmp19 * _tmp23;
|
||||
const Scalar _tmp36 = std::pow(_tmp13, Scalar(2)) * rot_var_ned(1, 0) +
|
||||
std::pow(_tmp16, Scalar(2)) * rot_var_ned(2, 0) +
|
||||
std::pow(_tmp3, Scalar(2)) * rot_var_ned(0, 0);
|
||||
const Scalar _tmp37 = -_tmp31 * _tmp32;
|
||||
const Scalar _tmp38 = _tmp28 * state(0, 0);
|
||||
const Scalar _tmp39 = -_tmp20 * _tmp22 + _tmp35 + _tmp38;
|
||||
const Scalar _tmp40 = (Scalar(1) / Scalar(2)) * state(0, 0);
|
||||
const Scalar _tmp41 = _tmp23 * _tmp32;
|
||||
const Scalar _tmp42 = _tmp28 * state(3, 0);
|
||||
const Scalar _tmp43 = _tmp30 * _tmp40 + _tmp41 - _tmp42;
|
||||
const Scalar _tmp44 = _tmp32 * _tmp40;
|
||||
const Scalar _tmp45 = _tmp21 + _tmp23 * _tmp36 + _tmp44;
|
||||
const Scalar _tmp46 = _tmp19 * _tmp31;
|
||||
const Scalar _tmp47 = _tmp22 * _tmp40 + _tmp42 - _tmp46;
|
||||
const Scalar _tmp48 = _tmp20 * _tmp30 + _tmp37 + _tmp38;
|
||||
const Scalar _tmp49 = _tmp19 * _tmp40;
|
||||
const Scalar _tmp50 = -_tmp31 * _tmp36 + _tmp33 + _tmp49;
|
||||
const Scalar _tmp51 = _tmp22 * _tmp31 + _tmp34 + _tmp49;
|
||||
const Scalar _tmp52 = -_tmp23 * _tmp30 + _tmp29 + _tmp44;
|
||||
const Scalar _tmp53 = _tmp36 * _tmp40 - _tmp41 + _tmp46;
|
||||
|
||||
// Output terms (1)
|
||||
if (q_cov_lower_triangle != nullptr) {
|
||||
matrix::Matrix<Scalar, 4, 4>& _q_cov_lower_triangle = (*q_cov_lower_triangle);
|
||||
|
||||
_q_cov_lower_triangle(0, 0) = -_tmp20 * (-_tmp20 * _tmp36 - _tmp35 + _tmp37) -
|
||||
_tmp23 * (_tmp21 - _tmp22 * _tmp23 - _tmp29) -
|
||||
_tmp31 * (-_tmp30 * _tmp31 - _tmp33 + _tmp34);
|
||||
_q_cov_lower_triangle(1, 0) = -_tmp20 * _tmp45 - _tmp23 * _tmp39 - _tmp31 * _tmp43;
|
||||
_q_cov_lower_triangle(2, 0) = -_tmp20 * _tmp50 - _tmp23 * _tmp47 - _tmp31 * _tmp48;
|
||||
_q_cov_lower_triangle(3, 0) = -_tmp20 * _tmp53 - _tmp23 * _tmp51 - _tmp31 * _tmp52;
|
||||
_q_cov_lower_triangle(0, 1) = 0;
|
||||
_q_cov_lower_triangle(1, 1) = -_tmp20 * _tmp39 + _tmp23 * _tmp45 + _tmp40 * _tmp43;
|
||||
_q_cov_lower_triangle(2, 1) = -_tmp20 * _tmp47 + _tmp23 * _tmp50 + _tmp40 * _tmp48;
|
||||
_q_cov_lower_triangle(3, 1) = -_tmp20 * _tmp51 + _tmp23 * _tmp53 + _tmp40 * _tmp52;
|
||||
_q_cov_lower_triangle(0, 2) = 0;
|
||||
_q_cov_lower_triangle(1, 2) = 0;
|
||||
_q_cov_lower_triangle(2, 2) = _tmp20 * _tmp48 - _tmp31 * _tmp50 + _tmp40 * _tmp47;
|
||||
_q_cov_lower_triangle(3, 2) = _tmp20 * _tmp52 - _tmp31 * _tmp53 + _tmp40 * _tmp51;
|
||||
_q_cov_lower_triangle(0, 3) = 0;
|
||||
_q_cov_lower_triangle(1, 3) = 0;
|
||||
_q_cov_lower_triangle(2, 3) = 0;
|
||||
_q_cov_lower_triangle(3, 3) = -_tmp23 * _tmp52 + _tmp31 * _tmp51 + _tmp40 * _tmp53;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
@@ -41,15 +41,15 @@ static_assert(sizeof(matrix::Vector<float, 24>) == sizeof(StateSample), "state v
|
||||
|
||||
struct IdxDof { unsigned idx; unsigned dof; };
|
||||
namespace State {
|
||||
static constexpr IdxDof quat_nominal{0, 4};
|
||||
static constexpr IdxDof vel{4, 3};
|
||||
static constexpr IdxDof pos{7, 3};
|
||||
static constexpr IdxDof gyro_bias{10, 3};
|
||||
static constexpr IdxDof accel_bias{13, 3};
|
||||
static constexpr IdxDof mag_I{16, 3};
|
||||
static constexpr IdxDof mag_B{19, 3};
|
||||
static constexpr IdxDof wind_vel{22, 2};
|
||||
static constexpr uint8_t size{24};
|
||||
static constexpr IdxDof quat_nominal{0, 3};
|
||||
static constexpr IdxDof vel{3, 3};
|
||||
static constexpr IdxDof pos{6, 3};
|
||||
static constexpr IdxDof gyro_bias{9, 3};
|
||||
static constexpr IdxDof accel_bias{12, 3};
|
||||
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};
|
||||
};
|
||||
}
|
||||
#endif // !EKF_STATE_H
|
||||
|
||||
@@ -1862,7 +1862,7 @@ void EKF2::PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu_sa
|
||||
_ekf.getPositionVariance().copyTo(odom.position_variance);
|
||||
|
||||
// orientation covariance
|
||||
_ekf.calcRotVecVariances().copyTo(odom.orientation_variance);
|
||||
_ekf.getQuaternionVariance().copyTo(odom.orientation_variance);
|
||||
|
||||
odom.reset_counter = _ekf.get_quat_reset_count()
|
||||
+ _ekf.get_velNE_reset_count() + _ekf.get_velD_reset_count()
|
||||
@@ -1949,8 +1949,9 @@ void EKF2::PublishStates(const hrt_abstime ×tamp)
|
||||
// publish estimator states
|
||||
estimator_states_s states;
|
||||
states.timestamp_sample = _ekf.time_delayed_us();
|
||||
states.n_states = Ekf::getNumberOfStates();
|
||||
_ekf.getStateAtFusionHorizonAsVector().copyTo(states.states);
|
||||
const auto state_vector = _ekf.state().vector();
|
||||
state_vector.copyTo(states.states);
|
||||
states.n_states = state_vector.size();
|
||||
_ekf.covariances_diagonal().copyTo(states.covariances);
|
||||
states.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
||||
_estimator_states_pub.publish(states);
|
||||
|
||||
@@ -37,14 +37,11 @@ add_subdirectory(sensor_simulator)
|
||||
add_subdirectory(test_helper)
|
||||
|
||||
px4_add_unit_gtest(SRC test_EKF_accelerometer.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
px4_add_unit_gtest(SRC test_EKF_attitude_variance.cpp LINKLIBS ecl_EKF ecl_test_helper)
|
||||
px4_add_unit_gtest(SRC test_EKF_airspeed_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
|
||||
px4_add_unit_gtest(SRC test_EKF_airspeed.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
px4_add_unit_gtest(SRC test_EKF_basics.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
px4_add_unit_gtest(SRC test_EKF_externalVision.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
|
||||
px4_add_unit_gtest(SRC test_EKF_flow.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
|
||||
px4_add_unit_gtest(SRC test_EKF_gyroscope.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
px4_add_unit_gtest(SRC test_EKF_opt_flow_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
|
||||
px4_add_unit_gtest(SRC test_EKF_fusionLogic.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
|
||||
px4_add_unit_gtest(SRC test_EKF_gps.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
|
||||
px4_add_unit_gtest(SRC test_EKF_gps_yaw.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
@@ -53,11 +50,9 @@ px4_add_unit_gtest(SRC test_EKF_height_fusion.cpp LINKLIBS ecl_EKF ecl_sensor_si
|
||||
px4_add_unit_gtest(SRC test_EKF_imuSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
px4_add_unit_gtest(SRC test_EKF_initialization.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
px4_add_unit_gtest(SRC test_EKF_mag.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
px4_add_unit_gtest(SRC test_EKF_mag_3d_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
|
||||
px4_add_unit_gtest(SRC test_EKF_mag_declination_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
|
||||
px4_add_unit_gtest(SRC test_EKF_measurementSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
px4_add_unit_gtest(SRC test_EKF_ringbuffer.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
px4_add_unit_gtest(SRC test_EKF_sideslip_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
|
||||
px4_add_unit_gtest(SRC test_EKF_terrain_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
|
||||
px4_add_unit_gtest(SRC test_EKF_utils.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
px4_add_unit_gtest(SRC test_EKF_withReplayData.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -52,17 +52,17 @@ void EkfLogger::writeState()
|
||||
_file << time;
|
||||
|
||||
if (_state_logging_enabled) {
|
||||
matrix::Vector<float, 24> state = _ekf->getStateAtFusionHorizonAsVector();
|
||||
auto state = _ekf->state().vector();
|
||||
|
||||
for (int i = 0; i < 24; i++) {
|
||||
for (unsigned i = 0; i < state.size(); i++) {
|
||||
_file << "," << std::setprecision(2) << state(i);
|
||||
}
|
||||
}
|
||||
|
||||
if (_variance_logging_enabled) {
|
||||
matrix::Vector<float, 24> variance = _ekf->covariances_diagonal();
|
||||
matrix::Vector<float, State::size> variance = _ekf->covariances_diagonal();
|
||||
|
||||
for (int i = 0; i < 24; i++) {
|
||||
for (unsigned i = 0; i < State::size; i++) {
|
||||
_file << "," << variance(i);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,146 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2022 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 <gtest/gtest.h>
|
||||
#include "EKF/ekf.h"
|
||||
#include "test_helper/comparison_helper.h"
|
||||
|
||||
#include "../EKF/python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h"
|
||||
#include "../EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h"
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
TEST(AirspeedFusionGenerated, SympyVsSymforce)
|
||||
{
|
||||
// Compare calculation of observation Jacobians and Kalman gains for sympy and symforce generated equations
|
||||
const float R_TAS = sq(1.5f);
|
||||
|
||||
const float vn = 9.0f;
|
||||
const float ve = 12.0f;
|
||||
const float vd = -1.5f;
|
||||
|
||||
const float vwn = -4.0f;
|
||||
const float vwe = 3.0f;
|
||||
|
||||
SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
|
||||
// First calculate observationjacobians and Kalman gains using sympy generated equations
|
||||
Vector24f Hfusion_sympy;
|
||||
Vector24f Kfusion_sympy;
|
||||
|
||||
{
|
||||
// Intermediate variables
|
||||
const float HK0 = vn - vwn;
|
||||
const float HK1 = ve - vwe;
|
||||
const float HK2 = powf(HK0, 2.f) + powf(HK1, 2.f) + powf(vd, 2.f);
|
||||
const float v_tas_pred = sqrtf(HK2); // predicted airspeed
|
||||
//const float HK3 = powf(HK2, -1.0F/2.0F);
|
||||
// calculation can be badly conditioned for very low airspeed values so don't fuse this time
|
||||
EXPECT_GT(v_tas_pred, 1.f);
|
||||
const float HK3 = 1.0f / v_tas_pred;
|
||||
const float HK4 = HK0 * HK3;
|
||||
const float HK5 = HK1 * HK3;
|
||||
const float HK6 = 1.0F / HK2;
|
||||
const float HK7 = HK0 * P(4, 6) - HK0 * P(6, 22) + HK1 * P(5, 6) - HK1 * P(6, 23) + P(6, 6) * vd;
|
||||
const float HK8 = HK1 * P(5, 23);
|
||||
const float HK9 = HK0 * P(4, 5) - HK0 * P(5, 22) + HK1 * P(5, 5) - HK8 + P(5, 6) * vd;
|
||||
const float HK10 = HK1 * HK6;
|
||||
const float HK11 = HK0 * P(4, 22);
|
||||
const float HK12 = HK0 * P(4, 4) - HK1 * P(4, 23) + HK1 * P(4, 5) - HK11 + P(4, 6) * vd;
|
||||
const float HK13 = HK0 * HK6;
|
||||
const float HK14 = -HK0 * P(22, 23) + HK0 * P(4, 23) - HK1 * P(23, 23) + HK8 + P(6, 23) * vd;
|
||||
const float HK15 = -HK0 * P(22, 22) - HK1 * P(22, 23) + HK1 * P(5, 22) + HK11 + P(6, 22) * vd;
|
||||
const float inn_var = (-HK10 * HK14 + HK10 * HK9 + HK12 * HK13 - HK13 * HK15 + HK6 * HK7 * vd + R_TAS);
|
||||
const float HK16 = HK3 / inn_var;
|
||||
|
||||
// Observation Jacobians
|
||||
SparseVector24f<4, 5, 6, 22, 23> Hfusion;
|
||||
Hfusion.at<4>() = HK4;
|
||||
Hfusion.at<5>() = HK5;
|
||||
Hfusion.at<6>() = HK3 * vd;
|
||||
Hfusion.at<22>() = -HK4;
|
||||
Hfusion.at<23>() = -HK5;
|
||||
|
||||
Vector24f Kfusion;
|
||||
|
||||
for (unsigned row = 0; row <= 3; row++) {
|
||||
Kfusion(row) = HK16 * (HK0 * P(4, row) - HK0 * P(row, 22) + HK1 * P(5, row) - HK1 * P(row, 23) + P(6, row) * vd);
|
||||
}
|
||||
|
||||
Kfusion(4) = HK12 * HK16;
|
||||
Kfusion(5) = HK16 * HK9;
|
||||
Kfusion(6) = HK16 * HK7;
|
||||
|
||||
for (unsigned row = 7; row <= 21; row++) {
|
||||
Kfusion(row) = HK16 * (HK0 * P(4, row) - HK0 * P(row, 22) + HK1 * P(5, row) - HK1 * P(row, 23) + P(6, row) * vd);
|
||||
}
|
||||
|
||||
Kfusion(22) = HK15 * HK16;
|
||||
Kfusion(23) = HK14 * HK16;
|
||||
|
||||
// save output
|
||||
Hfusion_sympy(4) = Hfusion.at<4>();
|
||||
Hfusion_sympy(5) = Hfusion.at<5>();
|
||||
Hfusion_sympy(6) = Hfusion.at<6>();
|
||||
Hfusion_sympy(22) = Hfusion.at<22>();
|
||||
Hfusion_sympy(23) = Hfusion.at<23>();
|
||||
Kfusion_sympy = Kfusion;
|
||||
}
|
||||
|
||||
// Then calculate observationjacobians and Kalman gains using symforce generated equations
|
||||
Vector24f Hfusion_symforce;
|
||||
Vector24f Kfusion_symforce;
|
||||
|
||||
{
|
||||
Vector24f state_vector{};
|
||||
state_vector(4) = vn;
|
||||
state_vector(5) = ve;
|
||||
state_vector(6) = vd;
|
||||
state_vector(22) = vwn;
|
||||
state_vector(23) = vwe;
|
||||
|
||||
float innov;
|
||||
float innov_var;
|
||||
|
||||
sym::ComputeAirspeedInnovAndInnovVar(state_vector, P, 0.f, R_TAS, FLT_EPSILON, &innov, &innov_var);
|
||||
sym::ComputeAirspeedHAndK(state_vector, P, innov_var, FLT_EPSILON, &Hfusion_symforce, &Kfusion_symforce);
|
||||
}
|
||||
|
||||
DiffRatioReport report = computeDiffRatioVector24f(Hfusion_sympy, Hfusion_symforce);
|
||||
EXPECT_LT(report.max_diff_fraction, 1e-5f) << "Airspeed Hfusion max diff fraction = " << report.max_diff_fraction <<
|
||||
" location index = " << report.max_row << " sympy = " << report.max_v1 << " symforce = " << report.max_v2;
|
||||
|
||||
report = computeDiffRatioVector24f(Kfusion_sympy, Kfusion_symforce);
|
||||
EXPECT_LT(report.max_diff_fraction, 1e-5f) << "Airspeed Kfusion max diff fraction = " << report.max_diff_fraction <<
|
||||
" location index = " << report.max_row << " sympy = " << report.max_v1 << " symforce = " << report.max_v2;
|
||||
}
|
||||
@@ -1,272 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2023 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 <gtest/gtest.h>
|
||||
#include "EKF/ekf.h"
|
||||
#include "test_helper/comparison_helper.h"
|
||||
|
||||
#include "../EKF/python/ekf_derivation/generated/quat_var_to_rot_var.h"
|
||||
#include "../EKF/python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h"
|
||||
#include "../EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h"
|
||||
#include "../EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h"
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
Vector3f calcRotVarMatlab(const Quatf &q, const SquareMatrix24f &P)
|
||||
{
|
||||
Vector3f rot_var_vec;
|
||||
float q0, q1, q2, q3;
|
||||
|
||||
if (q(0) >= 0.0f) {
|
||||
q0 = q(0);
|
||||
q1 = q(1);
|
||||
q2 = q(2);
|
||||
q3 = q(3);
|
||||
|
||||
} else {
|
||||
q0 = -q(0);
|
||||
q1 = -q(1);
|
||||
q2 = -q(2);
|
||||
q3 = -q(3);
|
||||
}
|
||||
|
||||
float t2 = q0 * q0;
|
||||
float t3 = acosf(q0);
|
||||
float t4 = -t2 + 1.0f;
|
||||
float t5 = t2 - 1.0f;
|
||||
|
||||
if ((t4 > 1e-9f) && (t5 < -1e-9f)) {
|
||||
float t6 = 1.0f / t5;
|
||||
float t7 = q1 * t6 * 2.0f;
|
||||
float t8 = 1.0f / powf(t4, 1.5f);
|
||||
float t9 = q0 * q1 * t3 * t8 * 2.0f;
|
||||
float t10 = t7 + t9;
|
||||
float t11 = 1.0f / sqrtf(t4);
|
||||
float t12 = q2 * t6 * 2.0f;
|
||||
float t13 = q0 * q2 * t3 * t8 * 2.0f;
|
||||
float t14 = t12 + t13;
|
||||
float t15 = q3 * t6 * 2.0f;
|
||||
float t16 = q0 * q3 * t3 * t8 * 2.0f;
|
||||
float t17 = t15 + t16;
|
||||
rot_var_vec(0) = t10 * (P(0, 0) * t10 + P(1, 0) * t3 * t11 * 2.0f) + t3 * t11 * (P(0, 1) * t10 + P(1,
|
||||
1) * t3 * t11 * 2.0f) * 2.0f;
|
||||
rot_var_vec(1) = t14 * (P(0, 0) * t14 + P(2, 0) * t3 * t11 * 2.0f) + t3 * t11 * (P(0, 2) * t14 + P(2,
|
||||
2) * t3 * t11 * 2.0f) * 2.0f;
|
||||
rot_var_vec(2) = t17 * (P(0, 0) * t17 + P(3, 0) * t3 * t11 * 2.0f) + t3 * t11 * (P(0, 3) * t17 + P(3,
|
||||
3) * t3 * t11 * 2.0f) * 2.0f;
|
||||
|
||||
} else {
|
||||
rot_var_vec = 4.0f * P.slice<3, 3>(1, 1).diag();
|
||||
}
|
||||
|
||||
return rot_var_vec;
|
||||
}
|
||||
|
||||
TEST(AttitudeVariance, matlabVsSymforce)
|
||||
{
|
||||
Quatf q(Eulerf(M_PI_F / 4.f, -M_PI_F / 6.f, M_PI_F));
|
||||
q = -q; // use non-canonical quaternion to cover special case
|
||||
|
||||
const SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
Vector3f rot_var_matlab = calcRotVarMatlab(q, P);
|
||||
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q(0);
|
||||
state_vector(1) = q(1);
|
||||
state_vector(2) = q(2);
|
||||
state_vector(3) = q(3);
|
||||
|
||||
Vector3f rot_var_symforce;
|
||||
sym::QuatVarToRotVar(state_vector, P, FLT_EPSILON, &rot_var_symforce);
|
||||
|
||||
EXPECT_EQ(rot_var_matlab, rot_var_symforce);
|
||||
}
|
||||
|
||||
TEST(AttitudeVariance, matlabVsSymforceZeroTilt)
|
||||
{
|
||||
Quatf q;
|
||||
|
||||
const SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
Vector3f rot_var_matlab = calcRotVarMatlab(q, P);
|
||||
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q(0);
|
||||
state_vector(1) = q(1);
|
||||
state_vector(2) = q(2);
|
||||
state_vector(3) = q(3);
|
||||
|
||||
Vector3f rot_var_symforce;
|
||||
sym::QuatVarToRotVar(state_vector, P, FLT_EPSILON, &rot_var_symforce);
|
||||
|
||||
EXPECT_EQ(rot_var_matlab, rot_var_symforce);
|
||||
|
||||
const Vector3f rot_var_true = 4.0f * P.slice<3, 3>(1, 1).diag(); // special case
|
||||
EXPECT_EQ(rot_var_symforce, rot_var_true);
|
||||
}
|
||||
|
||||
void increaseYawVar(const Vector24f &state_vector, SquareMatrix24f &P, const float yaw_var)
|
||||
{
|
||||
SquareMatrix<float, 4> q_cov;
|
||||
sym::RotVarNedToLowerTriangularQuatCov(state_vector, Vector3f(0.f, 0.f, yaw_var), &q_cov);
|
||||
q_cov.copyLowerToUpperTriangle();
|
||||
P.slice<4, 4>(0, 0) += q_cov;
|
||||
}
|
||||
|
||||
void setTiltVar(const Vector24f &state_vector, SquareMatrix24f &P, const float tilt_var)
|
||||
{
|
||||
SquareMatrix<float, 4> q_cov;
|
||||
sym::RotVarNedToLowerTriangularQuatCov(state_vector, Vector3f(tilt_var, tilt_var, 0.f), &q_cov);
|
||||
q_cov.copyLowerToUpperTriangle();
|
||||
P.slice<4, 4>(0, 0) = q_cov;
|
||||
}
|
||||
|
||||
float getYawVar(const Vector24f &state_vector, const SquareMatrix24f &P)
|
||||
{
|
||||
Vector24f H_YAW;
|
||||
float yaw_var = 0.f;
|
||||
sym::ComputeYaw312InnovVarAndH(state_vector, P, 0.f, FLT_EPSILON, &yaw_var, &H_YAW);
|
||||
return yaw_var;
|
||||
}
|
||||
|
||||
TEST(AttitudeVariance, increaseYawVarNoTilt)
|
||||
{
|
||||
Quatf q;
|
||||
SquareMatrix24f P;
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q(0);
|
||||
state_vector(1) = q(1);
|
||||
state_vector(2) = q(2);
|
||||
state_vector(3) = q(3);
|
||||
|
||||
const float yaw_var = radians(25.f);
|
||||
increaseYawVar(state_vector, P, yaw_var);
|
||||
|
||||
const float var_new = getYawVar(state_vector, P);
|
||||
|
||||
EXPECT_NEAR(var_new, yaw_var, 1e-6f);
|
||||
}
|
||||
|
||||
TEST(AttitudeVariance, increaseYawVarPitch90)
|
||||
{
|
||||
Quatf q(Eulerf(M_PI_F / 2.f, M_PI_F / 2.f, M_PI_F / 3.f));
|
||||
SquareMatrix24f P;
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q(0);
|
||||
state_vector(1) = q(1);
|
||||
state_vector(2) = q(2);
|
||||
state_vector(3) = q(3);
|
||||
|
||||
const float yaw_var = radians(25.f);
|
||||
increaseYawVar(state_vector, P, yaw_var);
|
||||
|
||||
const float var_new = getYawVar(state_vector, P);
|
||||
|
||||
EXPECT_NEAR(var_new, yaw_var, 1e-6f);
|
||||
}
|
||||
|
||||
TEST(AttitudeVariance, increaseYawWithTilt)
|
||||
{
|
||||
Quatf q(Eulerf(-M_PI_F, M_PI_F / 3.f, -M_PI_F / 5.f));
|
||||
SquareMatrix24f P;
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q(0);
|
||||
state_vector(1) = q(1);
|
||||
state_vector(2) = q(2);
|
||||
state_vector(3) = q(3);
|
||||
|
||||
// Set the yaw variance (from 0)
|
||||
const float yaw_var_1 = radians(25.f);
|
||||
increaseYawVar(state_vector, P, yaw_var_1);
|
||||
|
||||
const float var_1 = getYawVar(state_vector, P);
|
||||
EXPECT_NEAR(var_1, yaw_var_1, 1e-6f);
|
||||
|
||||
// Increase it even more
|
||||
const float yaw_var_2 = radians(12.f);
|
||||
increaseYawVar(state_vector, P, yaw_var_2);
|
||||
|
||||
const float var_2 = getYawVar(state_vector, P);
|
||||
EXPECT_NEAR(var_2, yaw_var_1 + yaw_var_2, 1e-6f);
|
||||
}
|
||||
|
||||
TEST(AttitudeVariance, setRotVarNoTilt)
|
||||
{
|
||||
Quatf q;
|
||||
SquareMatrix24f P;
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q(0);
|
||||
state_vector(1) = q(1);
|
||||
state_vector(2) = q(2);
|
||||
state_vector(3) = q(3);
|
||||
|
||||
const float tilt_var = radians(1.2f);
|
||||
setTiltVar(state_vector, P, tilt_var);
|
||||
|
||||
Vector3f rot_var;
|
||||
sym::QuatVarToRotVar(state_vector, P, FLT_EPSILON, &rot_var);
|
||||
|
||||
EXPECT_NEAR(rot_var(0), tilt_var, 1e-6f);
|
||||
EXPECT_NEAR(rot_var(1), tilt_var, 1e-6f);
|
||||
EXPECT_EQ(rot_var(2), 0.f);
|
||||
|
||||
// Compare against known values (special case)
|
||||
EXPECT_EQ(P(0, 0), 0.f);
|
||||
EXPECT_EQ(P(1, 1), 0.25f * tilt_var);
|
||||
EXPECT_EQ(P(2, 2), 0.25f * tilt_var);
|
||||
EXPECT_EQ(P(3, 3), 0.25f * 0.f); // no yaw var
|
||||
}
|
||||
|
||||
TEST(AttitudeVariance, setRotVarPitch90)
|
||||
{
|
||||
Quatf q(Eulerf(0.f, M_PI_F, 0.f));
|
||||
SquareMatrix24f P;
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q(0);
|
||||
state_vector(1) = q(1);
|
||||
state_vector(2) = q(2);
|
||||
state_vector(3) = q(3);
|
||||
|
||||
const float tilt_var = radians(1.2f);
|
||||
setTiltVar(state_vector, P, tilt_var);
|
||||
|
||||
Vector3f rot_var;
|
||||
sym::QuatVarToRotVar(state_vector, P, FLT_EPSILON, &rot_var);
|
||||
|
||||
// TODO: FIXME, due to the nonlinearity of the quaternion parameters,
|
||||
// setting the variance and getting it back is approximate.
|
||||
// The correct way would be to keep the uncertainty as a 3D vector in the tangent plane
|
||||
// instead of converting it to the parameter space
|
||||
// EXPECT_NEAR(rot_var(0), tilt_var, 1e-6f);
|
||||
// EXPECT_NEAR(rot_var(1), tilt_var, 1e-6f);
|
||||
// EXPECT_EQ(rot_var(2), 0.f);
|
||||
}
|
||||
@@ -38,83 +38,23 @@
|
||||
#include "../EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h"
|
||||
|
||||
using namespace matrix;
|
||||
using D = matrix::Dual<float, 4>;
|
||||
|
||||
void computeHDual(const Vector24f &state_vector, float yaw_offset, Vector24f &H)
|
||||
{
|
||||
matrix::Quaternion<D> q(D(state_vector(0), 0),
|
||||
D(state_vector(1), 1),
|
||||
D(state_vector(2), 2),
|
||||
D(state_vector(3), 3));
|
||||
|
||||
Dcm<D> R_to_earth(q);
|
||||
Vector3<D> ant_vec_bf(cos(D(yaw_offset)), sin(D(yaw_offset)), D());
|
||||
Vector3<D> ant_vec_ef = R_to_earth * ant_vec_bf;
|
||||
D meas_pred = atan2(ant_vec_ef(1), ant_vec_ef(0));
|
||||
|
||||
H.setZero();
|
||||
|
||||
for (int i = 0; i <= 3; i++) {
|
||||
H(i) = meas_pred.derivative(i);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(GnssYawFusionGenerated, SympyVsSymforce)
|
||||
{
|
||||
const float R_YAW = sq(0.3f);
|
||||
|
||||
const float yaw_offset = M_PI_F / 8.f;
|
||||
const float yaw = M_PI_F;
|
||||
|
||||
const Quatf q(Eulerf(M_PI_F / 4.f, M_PI_F / 3.f, M_PI_F));
|
||||
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q(0);
|
||||
state_vector(1) = q(1);
|
||||
state_vector(2) = q(2);
|
||||
state_vector(3) = q(3);
|
||||
|
||||
SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
|
||||
Vector24f H_dual;
|
||||
computeHDual(state_vector, yaw_offset, H_dual);
|
||||
|
||||
float meas_pred_symforce;
|
||||
float innov_var_symforce;
|
||||
Vector24f H_symforce;
|
||||
sym::ComputeGnssYawPredInnovVarAndH(state_vector, P, yaw_offset, R_YAW, FLT_EPSILON, &meas_pred_symforce,
|
||||
&innov_var_symforce, &H_symforce);
|
||||
|
||||
EXPECT_GT(innov_var_symforce, 50.f);
|
||||
EXPECT_LT(innov_var_symforce, 60.f);
|
||||
|
||||
EXPECT_EQ(H_symforce, H_dual);
|
||||
|
||||
// The predicted yaw is not exactly yaw + offset because roll and pitch are non-zero, but it's close to that
|
||||
EXPECT_NEAR(meas_pred_symforce, wrap_pi(yaw + yaw_offset), 0.05f);
|
||||
}
|
||||
|
||||
TEST(GnssYawFusionGenerated, SingularityPitch90)
|
||||
{
|
||||
// GIVEN: a vertically oriented antenna (antenna vector aligned with the Forward axis)
|
||||
const Quatf q(Eulerf(0.f, -M_PI_F / 2.f, 0.f));
|
||||
StateSample state{};
|
||||
state.quat_nominal = Eulerf(0.f, -M_PI_F / 2.f, 0.f);
|
||||
const float yaw_offset = M_PI_F;
|
||||
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q(0);
|
||||
state_vector(1) = q(1);
|
||||
state_vector(2) = q(2);
|
||||
state_vector(3) = q(3);
|
||||
|
||||
SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
SquareMatrixState P = createRandomCovarianceMatrix();
|
||||
const float R_YAW = sq(0.3f);
|
||||
|
||||
float meas_pred;
|
||||
float innov_var;
|
||||
Vector24f H;
|
||||
sym::ComputeGnssYawPredInnovVarAndH(state_vector, P, yaw_offset, R_YAW, FLT_EPSILON, &meas_pred,
|
||||
VectorState H;
|
||||
sym::ComputeGnssYawPredInnovVarAndH(state.vector(), P, yaw_offset, R_YAW, FLT_EPSILON, &meas_pred,
|
||||
&innov_var, &H);
|
||||
Vector24f K = P * H / innov_var;
|
||||
VectorState K = P * H / innov_var;
|
||||
|
||||
// THEN: the arctan is singular, the attitude isn't observable, so the innovation variance
|
||||
// is almost infinite and the Kalman gain goes to 0
|
||||
@@ -125,24 +65,19 @@ TEST(GnssYawFusionGenerated, SingularityPitch90)
|
||||
TEST(GnssYawFusionGenerated, SingularityRoll90)
|
||||
{
|
||||
// GIVEN: a vertically oriented antenna (antenna vector aligned with the Right axis)
|
||||
const Quatf q(Eulerf(-M_PI_F / 2.f, 0.f, 0.f));
|
||||
StateSample state{};
|
||||
state.quat_nominal = Eulerf(-M_PI_F / 2.f, 0.f, 0.f);
|
||||
const float yaw_offset = M_PI_F / 2.f;
|
||||
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q(0);
|
||||
state_vector(1) = q(1);
|
||||
state_vector(2) = q(2);
|
||||
state_vector(3) = q(3);
|
||||
|
||||
SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
SquareMatrixState P = createRandomCovarianceMatrix();
|
||||
const float R_YAW = sq(0.3f);
|
||||
|
||||
float meas_pred;
|
||||
float innov_var;
|
||||
Vector24f H;
|
||||
sym::ComputeGnssYawPredInnovVarAndH(state_vector, P, yaw_offset, R_YAW, FLT_EPSILON, &meas_pred,
|
||||
VectorState H;
|
||||
sym::ComputeGnssYawPredInnovVarAndH(state.vector(), P, yaw_offset, R_YAW, FLT_EPSILON, &meas_pred,
|
||||
&innov_var, &H);
|
||||
Vector24f K = P * H / innov_var;
|
||||
VectorState K = P * H / innov_var;
|
||||
|
||||
// THEN: the arctan is singular, the attitude isn't observable, so the innovation variance
|
||||
// is almost infinite and the Kalman gain goes to 0
|
||||
|
||||
@@ -333,10 +333,9 @@ TEST_F(EkfGpsHeadingTest, stopOnGround)
|
||||
_ekf_wrapper.setMagFuseTypeNone();
|
||||
|
||||
// WHEN: running without yaw aiding
|
||||
const matrix::Vector4f quat_variance_before = _ekf->getQuaternionVariance();
|
||||
const float yaw_variance_before = _ekf->getYawVar();
|
||||
_sensor_simulator.runSeconds(20.0);
|
||||
const matrix::Vector4f quat_variance_after = _ekf->getQuaternionVariance();
|
||||
|
||||
// THEN: the yaw variance increases
|
||||
EXPECT_GT(quat_variance_after(3), quat_variance_before(3));
|
||||
EXPECT_GT(_ekf->getYawVar(), yaw_variance_before);
|
||||
}
|
||||
|
||||
@@ -78,10 +78,10 @@ public:
|
||||
|
||||
void quaternionVarianceBigEnoughAfterOrientationInitialization(float quat_variance_limit = 0.00001f)
|
||||
{
|
||||
const matrix::Vector4f quat_variance = _ekf->getQuaternionVariance();
|
||||
const matrix::Vector3f quat_variance = _ekf->getQuaternionVariance();
|
||||
EXPECT_TRUE(quat_variance(0) > quat_variance_limit) << "quat_variance(3): " << quat_variance(0);
|
||||
EXPECT_TRUE(quat_variance(1) > quat_variance_limit) << "quat_variance(1): " << quat_variance(1);
|
||||
EXPECT_TRUE(quat_variance(2) > quat_variance_limit) << "quat_variance(2): " << quat_variance(2);
|
||||
EXPECT_TRUE(quat_variance(3) > quat_variance_limit) << "quat_variance(3): " << quat_variance(3);
|
||||
}
|
||||
|
||||
void yawVarianceBigEnoughAfterHeadingReset()
|
||||
@@ -209,12 +209,12 @@ TEST_F(EkfInitializationTest, gyroBias)
|
||||
if (fabsf(accel_bias(2)) > 0.3f) {
|
||||
|
||||
// Print state covariance and correlation matrices for debugging
|
||||
const matrix::SquareMatrix<float, 24> P = _ekf->covariances();
|
||||
const auto P = _ekf->covariances();
|
||||
|
||||
printf("State covariance:\n");
|
||||
|
||||
for (int i = 0; i <= 15; i++) {
|
||||
for (int j = 0; j <= 15; j++) {
|
||||
for (int i = 0; i <= State::size; i++) {
|
||||
for (int j = 0; j <= State::size; j++) {
|
||||
printf("%.3fe-9 ", ((double)P(i, j)) * 1e9);
|
||||
}
|
||||
|
||||
@@ -224,10 +224,10 @@ TEST_F(EkfInitializationTest, gyroBias)
|
||||
printf("State correlation:\n");
|
||||
printf("\t0\t1\t2\t3\t4\t5\t6\t7\t8\t9\t10\t11\t12\t13\t14\t15\n");
|
||||
|
||||
for (uint8_t i = 0; i <= 15; i++) {
|
||||
for (uint8_t i = 0; i <= State::size; i++) {
|
||||
printf("%d| ", i);
|
||||
|
||||
for (uint8_t j = 0; j <= 15; j++) {
|
||||
for (uint8_t j = 0; j <= State::size; j++) {
|
||||
float corr = sqrtf(fabsf(P(i, i) * P(j, j)));
|
||||
|
||||
if (corr > 0.0f) {
|
||||
|
||||
@@ -1,110 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2022 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 <gtest/gtest.h>
|
||||
#include "EKF/ekf.h"
|
||||
#include "test_helper/comparison_helper.h"
|
||||
|
||||
#include "../EKF/python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h"
|
||||
#include "../EKF/python/ekf_derivation/generated/compute_mag_y_innov_var_and_h.h"
|
||||
#include "../EKF/python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h"
|
||||
|
||||
using namespace matrix;
|
||||
using D = matrix::Dual<float, 24>;
|
||||
|
||||
void computeHDual(const Vector24f &state_vector, int axis, Vector24f &H)
|
||||
{
|
||||
matrix::Quaternion<D> q(D(state_vector(0), 0),
|
||||
D(state_vector(1), 1),
|
||||
D(state_vector(2), 2),
|
||||
D(state_vector(3), 3));
|
||||
|
||||
Vector3<D> mag_field_earth(D(state_vector(16), 16), D(state_vector(17), 17), D(state_vector(18), 18));
|
||||
Vector3<D> mag_bias_body(D(state_vector(19), 19), D(state_vector(20), 20), D(state_vector(21), 21));
|
||||
|
||||
Vector3<D> mag_pred = Dcm<D>(q).transpose() * mag_field_earth + mag_bias_body;
|
||||
|
||||
H.setZero();
|
||||
|
||||
for (int i = 0; i <= 23; i++) {
|
||||
H(i) = mag_pred(axis).derivative(i);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Mag3DFusionGenerated, symforceVsDual)
|
||||
{
|
||||
const Quatf q(Eulerf(-M_PI_F / 2.f, M_PI_F / 3.f, M_PI_F * 4.f / 5.f));
|
||||
const float q0 = q(0);
|
||||
const float q1 = q(1);
|
||||
const float q2 = q(2);
|
||||
const float q3 = q(3);
|
||||
|
||||
const float magN = 2.0f * ((float)randf() - 0.5f);
|
||||
const float magE = 2.0f * ((float)randf() - 0.5f);
|
||||
const float magD = 2.0f * ((float)randf() - 0.5f);
|
||||
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q0;
|
||||
state_vector(1) = q1;
|
||||
state_vector(2) = q2;
|
||||
state_vector(3) = q3;
|
||||
state_vector(16) = magN;
|
||||
state_vector(17) = magE;
|
||||
state_vector(18) = magD;
|
||||
|
||||
const float R_MAG = sq(0.05f);
|
||||
|
||||
SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
|
||||
Vector24f H_dual;
|
||||
Vector24f Hfusion_symforce;
|
||||
Vector3f mag_innov_var_symforce;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
computeHDual(state_vector, i, H_dual);
|
||||
|
||||
if (i == 0) {
|
||||
Vector3f innov;
|
||||
sym::ComputeMagInnovInnovVarAndHx(state_vector, P, Vector3f(), R_MAG, FLT_EPSILON, &innov, &mag_innov_var_symforce,
|
||||
&Hfusion_symforce);
|
||||
|
||||
} else if (i == 1) {
|
||||
sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &(mag_innov_var_symforce(i)), &Hfusion_symforce);
|
||||
|
||||
} else {
|
||||
sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &(mag_innov_var_symforce(i)), &Hfusion_symforce);
|
||||
}
|
||||
|
||||
EXPECT_EQ(Hfusion_symforce, H_dual);
|
||||
}
|
||||
}
|
||||
@@ -36,25 +36,26 @@
|
||||
#include "test_helper/comparison_helper.h"
|
||||
|
||||
#include "../EKF/python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h"
|
||||
#include "../EKF/python/ekf_derivation/generated/state.h"
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
TEST(MagDeclinationGenerated, declination90deg)
|
||||
{
|
||||
// GIVEN: an estimated mag declination of 90 degrees
|
||||
Vector24f state_vector{};
|
||||
state_vector(16) = 0.f; // North mag field
|
||||
state_vector(17) = 0.2f; // East mag field
|
||||
StateSample state{};
|
||||
state.mag_I(0) = 0.f; // North mag field
|
||||
state.mag_I(1) = 0.2f; // East mag field
|
||||
|
||||
const float R = sq(radians(sq(0.5f)));
|
||||
SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
SquareMatrixState P = createRandomCovarianceMatrix();
|
||||
|
||||
Vector24f H;
|
||||
VectorState H;
|
||||
float decl_pred;
|
||||
float innov_var;
|
||||
|
||||
const float decl = radians(90.f);
|
||||
sym::ComputeMagDeclinationPredInnovVarAndH(state_vector, P, R, FLT_EPSILON, &decl_pred, &innov_var, &H);
|
||||
sym::ComputeMagDeclinationPredInnovVarAndH(state.vector(), P, R, FLT_EPSILON, &decl_pred, &innov_var, &H);
|
||||
|
||||
// THEN: Even at the singularity point, atan2 is still defined
|
||||
EXPECT_TRUE(innov_var < 5000.f && innov_var > R) << "innov_var = " << innov_var;
|
||||
@@ -64,123 +65,21 @@ TEST(MagDeclinationGenerated, declination90deg)
|
||||
TEST(MagDeclinationGenerated, declinationUndefined)
|
||||
{
|
||||
// GIVEN: an undefined declination
|
||||
Vector24f state_vector{};
|
||||
state_vector(16) = 0.f; // North mag field
|
||||
state_vector(17) = 0.f; // East mag field
|
||||
StateSample state{};
|
||||
state.mag_I(0) = 0.f; // North mag field
|
||||
state.mag_I(1) = 0.f; // East mag field
|
||||
|
||||
const float R = sq(radians(sq(0.5f)));
|
||||
SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
SquareMatrixState P = createRandomCovarianceMatrix();
|
||||
|
||||
Vector24f H;
|
||||
VectorState H;
|
||||
float decl_pred;
|
||||
float innov_var;
|
||||
|
||||
const float decl = radians(0.f);
|
||||
sym::ComputeMagDeclinationPredInnovVarAndH(state_vector, P, R, FLT_EPSILON, &decl_pred, &innov_var, &H);
|
||||
sym::ComputeMagDeclinationPredInnovVarAndH(state.vector(), P, R, FLT_EPSILON, &decl_pred, &innov_var, &H);
|
||||
|
||||
// THEN: the innovation variance is gigantic but finite
|
||||
EXPECT_TRUE(PX4_ISFINITE(innov_var) && innov_var > R && innov_var > 1e9f) << "innov_var = " << innov_var;
|
||||
EXPECT_LT(fabsf(wrap_pi(decl_pred - decl)), 1e-6f);
|
||||
}
|
||||
|
||||
void sympyMagDeclInnovVarHAndK(float magN, float magE, const SquareMatrix24f &P, float R_DECL,
|
||||
float &innovation_variance, Vector24f &H, Vector24f &Kfusion)
|
||||
{
|
||||
const float h_field_min = 1e-3f;
|
||||
const float magN_sq = sq(magN);
|
||||
|
||||
if (magN_sq < sq(h_field_min)) {
|
||||
printf("bad numerical conditioning\n");
|
||||
return;
|
||||
}
|
||||
|
||||
const float HK0 = 1.0F / magN_sq;
|
||||
const float HK1 = HK0 * sq(magE) + 1.0F;
|
||||
const float HK2 = 1.0F / HK1;
|
||||
const float HK3 = 1.0F / magN;
|
||||
const float HK4 = HK2 * HK3;
|
||||
const float HK5 = HK3 * magE;
|
||||
const float HK6 = HK5 * P(16, 17) - P(17, 17);
|
||||
const float HK7 = 1.0F / sq(HK1);
|
||||
const float HK8 = HK5 * P(16, 16) - P(16, 17);
|
||||
innovation_variance = -HK0 * HK6 * HK7 + HK7 * HK8 * magE / (magN * magN_sq) + R_DECL;
|
||||
float HK9;
|
||||
|
||||
if (innovation_variance > R_DECL) {
|
||||
HK9 = HK4 / innovation_variance;
|
||||
|
||||
} else {
|
||||
printf("bad numerical conditioning\n");
|
||||
return;
|
||||
}
|
||||
|
||||
// Calculate the observation Jacobian
|
||||
// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
|
||||
float Hfusion[24] = {};
|
||||
Hfusion[16] = -HK0 * HK2 * magE;
|
||||
Hfusion[17] = HK4;
|
||||
|
||||
// Calculate the Kalman gains
|
||||
for (unsigned row = 0; row <= 15; row++) {
|
||||
Kfusion(row) = -HK9 * (HK5 * P(row, 16) - P(row, 17));
|
||||
}
|
||||
|
||||
Kfusion(16) = -HK8 * HK9;
|
||||
Kfusion(17) = -HK6 * HK9;
|
||||
|
||||
for (unsigned row = 18; row <= 23; row++) {
|
||||
Kfusion(row) = -HK9 * (HK5 * P(16, row) - P(17, row));
|
||||
}
|
||||
|
||||
for (int row = 0; row < 24; row++) {
|
||||
H(row) = Hfusion[row];
|
||||
}
|
||||
}
|
||||
|
||||
TEST(MagDeclinationGenerated, SympyVsSymforce)
|
||||
{
|
||||
const float R_DECL = sq(0.3f);
|
||||
const float mag_n = 0.08f;
|
||||
const float mag_e = -0.06f;
|
||||
|
||||
Vector24f state_vector{};
|
||||
state_vector(16) = mag_n;
|
||||
state_vector(17) = mag_e;
|
||||
|
||||
const float decl = M_PI_F;
|
||||
|
||||
SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
|
||||
Vector24f H_sympy;
|
||||
Vector24f H_symforce;
|
||||
Vector24f K_sympy;
|
||||
Vector24f K_symforce;
|
||||
float innov_sympy;
|
||||
float pred_symforce;
|
||||
float innov_var_sympy;
|
||||
float innov_var_symforce;
|
||||
|
||||
sympyMagDeclInnovVarHAndK(mag_n, mag_e, P, R_DECL, innov_var_sympy, H_sympy, K_sympy);
|
||||
innov_sympy = wrap_pi(std::atan2(mag_e, mag_n) - decl);
|
||||
sym::ComputeMagDeclinationPredInnovVarAndH(state_vector, P, R_DECL, FLT_EPSILON, &pred_symforce,
|
||||
&innov_var_symforce, &H_symforce);
|
||||
const float innov_symforce = wrap_pi(pred_symforce - decl);
|
||||
K_symforce = P * H_symforce / innov_var_symforce;
|
||||
|
||||
EXPECT_NEAR(innov_sympy, innov_symforce, 1e-5f);
|
||||
EXPECT_NEAR(innov_var_sympy, innov_var_symforce, 1e-2f); // Slightly different because of epsilon
|
||||
|
||||
DiffRatioReport report = computeDiffRatioVector24f(H_sympy, H_symforce);
|
||||
EXPECT_LT(report.max_diff_fraction, 2e-4f)
|
||||
<< "Max diff fraction = " << report.max_diff_fraction
|
||||
<< " location index = " << report.max_row
|
||||
<< " sympy = " << report.max_v1
|
||||
<< " symforce = " << report.max_v2;
|
||||
|
||||
report = computeDiffRatioVector24f(K_sympy, K_symforce);
|
||||
EXPECT_LT(report.max_diff_fraction, 2e-4f)
|
||||
<< "Max diff fraction = " << report.max_diff_fraction
|
||||
<< " location index = " << report.max_row
|
||||
<< " sympy = " << report.max_v1
|
||||
<< " symforce = " << report.max_v2;
|
||||
}
|
||||
|
||||
@@ -1,115 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2022 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 <gtest/gtest.h>
|
||||
#include "EKF/ekf.h"
|
||||
#include "test_helper/comparison_helper.h"
|
||||
|
||||
#include "../EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h"
|
||||
#include "../EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h"
|
||||
|
||||
using namespace matrix;
|
||||
using D = matrix::Dual<float, 7>;
|
||||
|
||||
void computeHDual(const Vector24f &state_vector, float range, int axis, Vector24f &H)
|
||||
{
|
||||
matrix::Quaternion<D> q(D(state_vector(0), 0),
|
||||
D(state_vector(1), 1),
|
||||
D(state_vector(2), 2),
|
||||
D(state_vector(3), 3));
|
||||
|
||||
// calculate the velocity of the sensor in the earth frame
|
||||
Vector3<D> vel_earth(D(state_vector(4), 4), D(state_vector(5), 5), D(state_vector(6), 6));
|
||||
|
||||
// rotate into body frame
|
||||
Vector3<D> vel_body = Dcm<D>(q).transpose() * vel_earth;
|
||||
matrix::Vector2<D> predicted_flow;
|
||||
predicted_flow(0) = vel_body(1) / range;
|
||||
predicted_flow(1) = -vel_body(0) / range;
|
||||
|
||||
H.setZero();
|
||||
|
||||
for (int i = 0; i <= 6; i++) {
|
||||
H(i) = predicted_flow(axis).derivative(i);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(OptFlowFusionGenerated, symforceVsDual)
|
||||
{
|
||||
// Compare calculation of observation Jacobians
|
||||
const Quatf q(Eulerf(M_PI_F / 4.f, -M_PI_F / 6.f, M_PI_F));
|
||||
const float q0 = q(0);
|
||||
const float q1 = q(1);
|
||||
const float q2 = q(2);
|
||||
const float q3 = q(3);
|
||||
|
||||
const float vn = 5.f;
|
||||
const float ve = 0.f;
|
||||
const float vd = -1.5f;
|
||||
|
||||
const float range = 5.0f;
|
||||
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q0;
|
||||
state_vector(1) = q1;
|
||||
state_vector(2) = q2;
|
||||
state_vector(3) = q3;
|
||||
state_vector(4) = vn;
|
||||
state_vector(5) = ve;
|
||||
state_vector(6) = vd;
|
||||
|
||||
const float R_LOS = sq(0.15f);
|
||||
|
||||
SquareMatrix24f P;
|
||||
|
||||
Vector24f H;
|
||||
Vector24f H_dual;
|
||||
Vector2f innov_var;
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
computeHDual(state_vector, range, i, H_dual);
|
||||
|
||||
if (i == 0) {
|
||||
sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
|
||||
|
||||
} else {
|
||||
sym::ComputeFlowYInnovVarAndH(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var(1), &H);
|
||||
}
|
||||
|
||||
// Both derivations are equivalent
|
||||
EXPECT_EQ(H, H_dual);
|
||||
|
||||
// Since the state variance is 0, the observation variance is the innovation variance
|
||||
EXPECT_FLOAT_EQ(innov_var(i), R_LOS);
|
||||
}
|
||||
}
|
||||
@@ -1,105 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2022 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 <gtest/gtest.h>
|
||||
#include "EKF/ekf.h"
|
||||
#include "test_helper/comparison_helper.h"
|
||||
|
||||
#include "../EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h"
|
||||
#include "../EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h"
|
||||
|
||||
using namespace matrix;
|
||||
using D = matrix::Dual<float, 24>;
|
||||
|
||||
void computeHDual(const Vector24f &state_vector, Vector24f &H)
|
||||
{
|
||||
matrix::Quaternion<D> q(D(state_vector(0), 0),
|
||||
D(state_vector(1), 1),
|
||||
D(state_vector(2), 2),
|
||||
D(state_vector(3), 3));
|
||||
|
||||
Vector3<D> vel_earth(D(state_vector(4), 4), D(state_vector(5), 5), D(state_vector(6), 6));
|
||||
Vector3<D> wind_earth(D(state_vector(22), 22), D(state_vector(23), 23), D());
|
||||
Vector3<D> vel_rel_body = Dcm<D>(q).transpose() * (vel_earth - wind_earth);
|
||||
D sideslip_pred = vel_rel_body(1) / vel_rel_body(0);
|
||||
|
||||
H.setZero();
|
||||
|
||||
for (int i = 0; i <= 23; i++) {
|
||||
H(i) = sideslip_pred.derivative(i);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(SideslipFusionGenerated, symforceVsDual)
|
||||
{
|
||||
// Compare calculation of observation Jacobians and Kalman gains for sympy and symforce generated equations
|
||||
const float R_BETA = sq(2.5f);
|
||||
|
||||
const Quatf q(Eulerf(-M_PI_F / 2.f, M_PI_F / 3.f, M_PI_F * 4.f / 5.f));
|
||||
const float q0 = q(0);
|
||||
const float q1 = q(1);
|
||||
const float q2 = q(2);
|
||||
const float q3 = q(3);
|
||||
|
||||
const float vn = 9.0f;
|
||||
const float ve = 12.0f;
|
||||
const float vd = -1.5f;
|
||||
|
||||
const float vwn = -4.0f;
|
||||
const float vwe = 3.0f;
|
||||
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q0;
|
||||
state_vector(1) = q1;
|
||||
state_vector(2) = q2;
|
||||
state_vector(3) = q3;
|
||||
state_vector(4) = vn;
|
||||
state_vector(5) = ve;
|
||||
state_vector(6) = vd;
|
||||
state_vector(22) = vwn;
|
||||
state_vector(23) = vwe;
|
||||
|
||||
SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
|
||||
float innov;
|
||||
float innov_var;
|
||||
Vector24f H_symforce;
|
||||
Vector24f K_symforce;
|
||||
Vector24f H_dual;
|
||||
|
||||
sym::ComputeSideslipInnovAndInnovVar(state_vector, P, R_BETA, FLT_EPSILON, &innov, &innov_var);
|
||||
sym::ComputeSideslipHAndK(state_vector, P, innov_var, FLT_EPSILON, &H_symforce, &K_symforce);
|
||||
computeHDual(state_vector, H_dual);
|
||||
|
||||
EXPECT_TRUE((H_symforce - H_dual).max() < 1e-3f) << H_symforce << H_dual;
|
||||
}
|
||||
@@ -46,26 +46,21 @@ TEST(YawFusionGenerated, singularityYawEquivalence)
|
||||
{
|
||||
// GIVEN: an attitude that should give a singularity when transforming the
|
||||
// rotation matrix to Euler yaw
|
||||
const Quatf q(Eulerf(M_PI_F, 0.f, M_PI_F));
|
||||
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q(0);
|
||||
state_vector(1) = q(1);
|
||||
state_vector(2) = q(2);
|
||||
state_vector(3) = q(3);
|
||||
StateSample state{};
|
||||
state.quat_nominal = Eulerf(M_PI_F, 0.f, M_PI_F);
|
||||
|
||||
const float R = sq(radians(10.f));
|
||||
SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
SquareMatrixState P = createRandomCovarianceMatrix();
|
||||
|
||||
Vector24f H_a;
|
||||
Vector24f H_b;
|
||||
VectorState H_a;
|
||||
VectorState H_b;
|
||||
float innov_var_a;
|
||||
float innov_var_b;
|
||||
|
||||
// WHEN: computing the innovation variance and H using two different
|
||||
// alternate forms (one is singular at pi/2 and the other one at 0)
|
||||
sym::ComputeYaw321InnovVarAndH(state_vector, P, R, FLT_EPSILON, &innov_var_a, &H_a);
|
||||
sym::ComputeYaw321InnovVarAndHAlternate(state_vector, P, R, FLT_EPSILON, &innov_var_b, &H_b);
|
||||
sym::ComputeYaw321InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var_a, &H_a);
|
||||
sym::ComputeYaw321InnovVarAndHAlternate(state.vector(), P, R, FLT_EPSILON, &innov_var_b, &H_b);
|
||||
|
||||
// THEN: Even at the singularity point, the result is still correct, thanks to epsilon
|
||||
EXPECT_TRUE(isEqual(H_a, H_b));
|
||||
@@ -76,24 +71,19 @@ TEST(YawFusionGenerated, singularityYawEquivalence)
|
||||
TEST(YawFusionGenerated, gimbalLock321vs312)
|
||||
{
|
||||
// GIVEN: an attitude at gimbal lock position
|
||||
const Quatf q(Eulerf(0.f, -M_PI_F / 2.f, M_PI_F));
|
||||
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q(0);
|
||||
state_vector(1) = q(1);
|
||||
state_vector(2) = q(2);
|
||||
state_vector(3) = q(3);
|
||||
StateSample state{};
|
||||
state.quat_nominal = Eulerf(0.f, -M_PI_F / 2.f, M_PI_F);
|
||||
|
||||
const float R = sq(radians(10.f));
|
||||
SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
SquareMatrixState P = createRandomCovarianceMatrix();
|
||||
|
||||
Vector24f H_321;
|
||||
Vector24f H_312;
|
||||
VectorState H_321;
|
||||
VectorState H_312;
|
||||
float innov_var_321;
|
||||
float innov_var_312;
|
||||
sym::ComputeYaw321InnovVarAndH(state_vector, P, R, FLT_EPSILON, &innov_var_321, &H_321);
|
||||
sym::ComputeYaw321InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var_321, &H_321);
|
||||
|
||||
sym::ComputeYaw312InnovVarAndH(state_vector, P, R, FLT_EPSILON, &innov_var_312, &H_312);
|
||||
sym::ComputeYaw312InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var_312, &H_312);
|
||||
|
||||
// THEN: both computation are not equivalent, 321 is undefined but 312 is valid
|
||||
EXPECT_FALSE(isEqual(H_321, H_312));
|
||||
@@ -104,27 +94,23 @@ TEST(YawFusionGenerated, gimbalLock321vs312)
|
||||
TEST(YawFusionGenerated, positiveVarianceAllOrientations)
|
||||
{
|
||||
const float R = sq(radians(10.f));
|
||||
SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
SquareMatrixState P = createRandomCovarianceMatrix();
|
||||
|
||||
Vector24f H;
|
||||
VectorState H;
|
||||
float innov_var;
|
||||
|
||||
// GIVEN: all orientations (90 deg steps)
|
||||
for (float yaw = 0.f; yaw < 2.f * M_PI_F; yaw += M_PI_F / 2.f) {
|
||||
for (float pitch = 0.f; pitch < 2.f * M_PI_F; pitch += M_PI_F / 2.f) {
|
||||
for (float roll = 0.f; roll < 2.f * M_PI_F; roll += M_PI_F / 2.f) {
|
||||
const Quatf q(Eulerf(roll, pitch, yaw));
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q(0);
|
||||
state_vector(1) = q(1);
|
||||
state_vector(2) = q(2);
|
||||
state_vector(3) = q(3);
|
||||
StateSample state{};
|
||||
state.quat_nominal = Eulerf(roll, pitch, yaw);
|
||||
|
||||
if (shouldUse321RotationSequence(Dcmf(q))) {
|
||||
sym::ComputeYaw321InnovVarAndH(state_vector, P, R, FLT_EPSILON, &innov_var, &H);
|
||||
if (shouldUse321RotationSequence(Dcmf(state.quat_nominal))) {
|
||||
sym::ComputeYaw321InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var, &H);
|
||||
|
||||
} else {
|
||||
sym::ComputeYaw312InnovVarAndH(state_vector, P, R, FLT_EPSILON, &innov_var, &H);
|
||||
sym::ComputeYaw312InnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var, &H);
|
||||
}
|
||||
|
||||
// THEN: the innovation variance must be positive and finite
|
||||
@@ -137,74 +123,3 @@ TEST(YawFusionGenerated, positiveVarianceAllOrientations)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
using D = matrix::Dual<float, 4>;
|
||||
|
||||
void computeHDual321(const Vector24f &state_vector, Vector24f &H)
|
||||
{
|
||||
matrix::Quaternion<D> q(D(state_vector(0), 0),
|
||||
D(state_vector(1), 1),
|
||||
D(state_vector(2), 2),
|
||||
D(state_vector(3), 3));
|
||||
|
||||
Dcm<D> R_to_earth(q);
|
||||
D yaw_pred = atan2(R_to_earth(1, 0), R_to_earth(0, 0));
|
||||
|
||||
H.setZero();
|
||||
|
||||
for (int i = 0; i <= 3; i++) {
|
||||
H(i) = yaw_pred.derivative(i);
|
||||
}
|
||||
}
|
||||
|
||||
void computeHDual312(const Vector24f &state_vector, Vector24f &H)
|
||||
{
|
||||
matrix::Quaternion<D> q(D(state_vector(0), 0),
|
||||
D(state_vector(1), 1),
|
||||
D(state_vector(2), 2),
|
||||
D(state_vector(3), 3));
|
||||
|
||||
Dcm<D> R_to_earth(q);
|
||||
D yaw_pred = atan2(-R_to_earth(0, 1), R_to_earth(1, 1));
|
||||
|
||||
H.setZero();
|
||||
|
||||
for (int i = 0; i <= 3; i++) {
|
||||
H(i) = yaw_pred.derivative(i);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(YawFusionGenerated, symforceVsDual)
|
||||
{
|
||||
const float R = sq(radians(10.f));
|
||||
SquareMatrix24f P = createRandomCovarianceMatrix24f();
|
||||
|
||||
Vector24f H_dual;
|
||||
Vector24f Hfusion_symforce;
|
||||
float innov_var;
|
||||
|
||||
// GIVEN: all orientations (90 deg steps)
|
||||
for (float yaw = 0.f; yaw < 2.f * M_PI_F; yaw += M_PI_F / 2.f) {
|
||||
for (float pitch = 0.f; pitch < 2.f * M_PI_F; pitch += M_PI_F / 2.f) {
|
||||
for (float roll = 0.f; roll < 2.f * M_PI_F; roll += M_PI_F / 2.f) {
|
||||
const Quatf q(Eulerf(roll, pitch, yaw));
|
||||
Vector24f state_vector{};
|
||||
state_vector(0) = q(0);
|
||||
state_vector(1) = q(1);
|
||||
state_vector(2) = q(2);
|
||||
state_vector(3) = q(3);
|
||||
|
||||
if (shouldUse321RotationSequence(Dcmf(q))) {
|
||||
sym::ComputeYaw321InnovVarAndH(state_vector, P, R, FLT_EPSILON, &innov_var, &Hfusion_symforce);
|
||||
computeHDual321(state_vector, H_dual);
|
||||
|
||||
} else {
|
||||
sym::ComputeYaw312InnovVarAndH(state_vector, P, R, FLT_EPSILON, &innov_var, &Hfusion_symforce);
|
||||
computeHDual312(state_vector, H_dual);
|
||||
}
|
||||
|
||||
EXPECT_EQ(Hfusion_symforce, H_dual);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -38,12 +38,12 @@ float randf()
|
||||
return (float)rand() / (float)RAND_MAX;
|
||||
}
|
||||
|
||||
SquareMatrix24f createRandomCovarianceMatrix24f()
|
||||
SquareMatrixState createRandomCovarianceMatrix()
|
||||
{
|
||||
// Create a symmetric square matrix
|
||||
SquareMatrix24f P;
|
||||
SquareMatrixState P;
|
||||
|
||||
for (int col = 0; col <= 23; col++) {
|
||||
for (int col = 0; col < State::size; col++) {
|
||||
for (int row = 0; row <= col; row++) {
|
||||
if (row == col) {
|
||||
P(row, col) = randf();
|
||||
@@ -59,61 +59,3 @@ SquareMatrix24f createRandomCovarianceMatrix24f()
|
||||
|
||||
return P;
|
||||
}
|
||||
|
||||
DiffRatioReport computeDiffRatioVector24f(const Vector24f &v1, const Vector24f &v2)
|
||||
{
|
||||
DiffRatioReport report = {};
|
||||
|
||||
for (int row = 0; row < 24; row++) {
|
||||
float diff_fraction;
|
||||
|
||||
if (fabsf(v1(row)) > FLT_EPSILON) {
|
||||
diff_fraction = fabsf(v2(row) - v1(row)) / fabsf(v1(row));
|
||||
|
||||
} else if (fabsf(v2(row)) > FLT_EPSILON) {
|
||||
diff_fraction = fabsf(v2(row) - v1(row)) / fabsf(v2(row));
|
||||
|
||||
} else {
|
||||
diff_fraction = 0.0f;
|
||||
}
|
||||
|
||||
if (diff_fraction > report.max_diff_fraction) {
|
||||
report.max_diff_fraction = diff_fraction;
|
||||
report.max_row = row;
|
||||
report.max_v1 = v1(row);
|
||||
report.max_v2 = v2(row);
|
||||
}
|
||||
}
|
||||
|
||||
return report;
|
||||
}
|
||||
|
||||
DiffRatioReport computeDiffRatioSquareMatrix24f(const SquareMatrix24f &m1, const SquareMatrix24f &m2)
|
||||
{
|
||||
DiffRatioReport report = {};
|
||||
|
||||
for (int row = 0; row < 24; row++) {
|
||||
for (int col = 0; col < 24; col++) {
|
||||
float diff_fraction;
|
||||
|
||||
if (fabsf(m1(row, col)) > FLT_EPSILON) {
|
||||
diff_fraction = fabsf(m2(row, col) - m1(row, col)) / fabsf(m1(row, col));
|
||||
|
||||
} else if (fabsf(m2(row, col)) > FLT_EPSILON) {
|
||||
diff_fraction = fabsf(m2(row, col) - m1(row, col)) / fabsf(m2(row, col));
|
||||
|
||||
} else {
|
||||
diff_fraction = 0.0f;
|
||||
}
|
||||
|
||||
if (diff_fraction > report.max_diff_fraction) {
|
||||
report.max_diff_fraction = diff_fraction;
|
||||
report.max_row = row;
|
||||
report.max_v1 = m1(row, col);
|
||||
report.max_v2 = m2(row, col);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return report;
|
||||
}
|
||||
|
||||
@@ -36,24 +36,11 @@
|
||||
|
||||
#include "EKF/ekf.h"
|
||||
|
||||
typedef matrix::Vector<float, 24> Vector24f;
|
||||
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
|
||||
template<int ... Idxs>
|
||||
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
|
||||
|
||||
struct DiffRatioReport {
|
||||
float max_diff_fraction;
|
||||
float max_row;
|
||||
float max_v1;
|
||||
float max_v2;
|
||||
};
|
||||
typedef matrix::Vector<float, State::size> VectorState;
|
||||
typedef matrix::SquareMatrix<float, State::size> SquareMatrixState;
|
||||
|
||||
float randf();
|
||||
|
||||
// Create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
|
||||
SquareMatrix24f createRandomCovarianceMatrix24f();
|
||||
|
||||
// Find largest element-wise difference as a fraction of v1 or v2
|
||||
DiffRatioReport computeDiffRatioVector24f(const Vector24f &v1, const Vector24f &v2);
|
||||
DiffRatioReport computeDiffRatioSquareMatrix24f(const SquareMatrix24f &m1, const SquareMatrix24f &m2);
|
||||
SquareMatrixState createRandomCovarianceMatrix();
|
||||
#endif
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user