forked from Archive/PX4-Autopilot
ekf2-gravity: nomalize gravity fusion and proper sequential fusion
This commit is contained in:
bresch
Daniel Agar
parent
d624fbba07
commit
7c7a3c117a
|
|
@ -358,7 +358,7 @@ class SourceParser(object):
|
|||
'deg', 'deg*1e7', 'deg/s', 'deg/s^2',
|
||||
'celcius', 'gauss', 'gauss/s', 'gauss^2',
|
||||
'hPa', 'kg', 'kg/m^2', 'kg m^2', 'kg/m^3',
|
||||
'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', '1/s/sqrt(Hz)', 'm/s/rad',
|
||||
'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', '1/s/sqrt(Hz)', 'm/s/rad', 'g',
|
||||
'Ohm', 'V', 'A',
|
||||
'us', 'ms', 's',
|
||||
'S', 'A/%', '(m/s^2)^2', 'm/m', 'tan(rad)^2', '(m/s)^2', 'm/rad',
|
||||
|
|
|
|||
|
|
@ -40,14 +40,16 @@
|
|||
*/
|
||||
|
||||
#include "ekf.h"
|
||||
#include <ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h>
|
||||
#include <ekf_derivation/generated/compute_gravity_xyz_innov_var_and_hx.h>
|
||||
#include <ekf_derivation/generated/compute_gravity_y_innov_var_and_h.h>
|
||||
#include <ekf_derivation/generated/compute_gravity_z_innov_var_and_h.h>
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
void Ekf::controlGravityFusion(const imuSample &imu)
|
||||
{
|
||||
// get raw accelerometer reading at delayed horizon and expected measurement noise (gaussian)
|
||||
const Vector3f measurement = imu.delta_vel / imu.delta_vel_dt - _state.accel_bias;
|
||||
const Vector3f measurement = Vector3f(imu.delta_vel / imu.delta_vel_dt - _state.accel_bias).unit();
|
||||
const float measurement_var = math::max(sq(_params.gravity_noise), sq(0.01f));
|
||||
|
||||
const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
|
||||
|
|
@ -60,12 +62,11 @@ void Ekf::controlGravityFusion(const imuSample &imu)
|
|||
&& !isHorizontalAidingActive();
|
||||
|
||||
// calculate kalman gains and innovation variances
|
||||
Vector3f innovation; // innovation of the last gravity fusion observation (m/s**2)
|
||||
Vector3f innovation = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, -1.f)) - measurement;
|
||||
Vector3f innovation_variance;
|
||||
VectorState Kx, Ky, Kz; // Kalman gain vectors
|
||||
sym::ComputeGravityInnovVarAndKAndH(
|
||||
_state.vector(), P, measurement, measurement_var, FLT_EPSILON,
|
||||
&innovation, &innovation_variance, &Kx, &Ky, &Kz);
|
||||
const auto state_vector = _state.vector();
|
||||
VectorState H;
|
||||
sym::ComputeGravityXyzInnovVarAndHx(state_vector, P, measurement_var, &innovation_variance, &H);
|
||||
|
||||
// fill estimator aid source status
|
||||
resetEstimatorAidStatus(_aid_src_gravity);
|
||||
|
|
@ -82,16 +83,40 @@ void Ekf::controlGravityFusion(const imuSample &imu)
|
|||
float innovation_gate = 0.25f;
|
||||
setEstimatorAidStatusTestRatio(_aid_src_gravity, innovation_gate);
|
||||
|
||||
const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2];
|
||||
bool fused[3] {false, false, false};
|
||||
|
||||
if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) {
|
||||
// perform fusion for each axis
|
||||
_aid_src_gravity.fused = measurementUpdate(Kx, innovation_variance(0), innovation(0))
|
||||
&& measurementUpdate(Ky, innovation_variance(1), innovation(1))
|
||||
&& measurementUpdate(Kz, innovation_variance(2), innovation(2));
|
||||
// update the states and covariance using sequential fusion
|
||||
for (uint8_t index = 0; index <= 2; index++) {
|
||||
// Calculate Kalman gains and observation jacobians
|
||||
if (index == 0) {
|
||||
// everything was already computed above
|
||||
|
||||
if (_aid_src_gravity.fused) {
|
||||
_aid_src_gravity.time_last_fuse = imu.time_us;
|
||||
} else if (index == 1) {
|
||||
// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
|
||||
sym::ComputeGravityYInnovVarAndH(state_vector, P, measurement_var, &_aid_src_gravity.innovation_variance[index], &H);
|
||||
|
||||
// recalculate innovation using the updated state
|
||||
_aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, -1.f))(index) - measurement(index);
|
||||
|
||||
} else if (index == 2) {
|
||||
// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
|
||||
sym::ComputeGravityZInnovVarAndH(state_vector, P, measurement_var, &_aid_src_gravity.innovation_variance[index], &H);
|
||||
|
||||
// recalculate innovation using the updated state
|
||||
_aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, -1.f))(index) - measurement(index);
|
||||
}
|
||||
|
||||
VectorState K = P * H / _aid_src_gravity.innovation_variance[index];
|
||||
|
||||
const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2];
|
||||
|
||||
if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) {
|
||||
fused[index] = measurementUpdate(K, _aid_src_gravity.innovation_variance[index], _aid_src_gravity.innovation[index]);
|
||||
}
|
||||
}
|
||||
|
||||
if (fused[0] && fused[1] && fused[2]) {
|
||||
_aid_src_gravity.fused = true;
|
||||
_aid_src_gravity.time_last_fuse = imu.time_us;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -607,35 +607,64 @@ def compute_drag_y_innov_var_and_k(
|
|||
|
||||
return (innov_var, K)
|
||||
|
||||
def compute_gravity_innov_var_and_k_and_h(
|
||||
state: VState,
|
||||
P: MTangent,
|
||||
meas: sf.V3,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.V3, sf.V3, VTangent, VTangent, VTangent):
|
||||
|
||||
state = vstate_to_state(state)
|
||||
def predict_gravity_direction(state: State):
|
||||
# get transform from earth to body frame
|
||||
R_to_body = state["quat_nominal"].inverse()
|
||||
|
||||
# the innovation is the error between measured acceleration
|
||||
# and predicted (body frame), assuming no body acceleration
|
||||
meas_pred = R_to_body * sf.Matrix([0,0,-9.80665])
|
||||
innov = meas_pred - 9.80665 * meas.normalized(epsilon=epsilon)
|
||||
return R_to_body * sf.Matrix([0,0,-1])
|
||||
|
||||
def compute_gravity_xyz_innov_var_and_hx(
|
||||
state: VState,
|
||||
P: MTangent,
|
||||
R: sf.Scalar
|
||||
) -> (sf.V3, VTangent):
|
||||
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_gravity_direction(state)
|
||||
|
||||
# initialize outputs
|
||||
innov_var = sf.V3()
|
||||
K = [None] * 3
|
||||
H = [None] * 3
|
||||
|
||||
# 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)
|
||||
innov_var[i] = (H * P * H.T + R)[0,0]
|
||||
K[i] = P * H.T / innov_var[i]
|
||||
H[i] = sf.V1(meas_pred[i]).jacobian(state)
|
||||
innov_var[i] = (H[i] * P * H[i].T + R)[0,0]
|
||||
|
||||
return (innov, innov_var, K[0], K[1], K[2])
|
||||
return (innov_var, H[0].T)
|
||||
|
||||
def compute_gravity_y_innov_var_and_h(
|
||||
state: VState,
|
||||
P: MTangent,
|
||||
R: sf.Scalar
|
||||
) -> (sf.V3, VTangent, VTangent, VTangent):
|
||||
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_gravity_direction(state)
|
||||
|
||||
# calculate observation jacobian (H), kalman gain (K), and innovation variance (S)
|
||||
H = sf.V1(meas_pred[1]).jacobian(state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
|
||||
def compute_gravity_z_innov_var_and_h(
|
||||
state: VState,
|
||||
P: MTangent,
|
||||
R: sf.Scalar
|
||||
) -> (sf.V3, VTangent, VTangent, VTangent):
|
||||
|
||||
state = vstate_to_state(state)
|
||||
meas_pred = predict_gravity_direction(state)
|
||||
|
||||
# calculate observation jacobian (H), kalman gain (K), and innovation variance (S)
|
||||
H = sf.V1(meas_pred[2]).jacobian(state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
|
||||
print("Derive EKF2 equations...")
|
||||
generate_px4_function(predict_covariance, output_names=None)
|
||||
|
|
@ -662,6 +691,8 @@ generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=["
|
|||
generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
|
||||
generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])
|
||||
generate_px4_function(compute_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(compute_gravity_xyz_innov_var_and_hx, output_names=["innov_var", "Hx"])
|
||||
generate_px4_function(compute_gravity_y_innov_var_and_h, output_names=["innov_var", "Hy"])
|
||||
generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_var", "Hz"])
|
||||
|
||||
generate_px4_state(State, tangent_idx)
|
||||
|
|
|
|||
|
|
@ -1,191 +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: compute_gravity_innov_var_and_k_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* meas: Matrix31
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov: Matrix31
|
||||
* innov_var: Matrix31
|
||||
* 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, 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, 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 (31)
|
||||
const Scalar _tmp0 = 2 * state(2, 0);
|
||||
const Scalar _tmp1 = _tmp0 * state(0, 0);
|
||||
const Scalar _tmp2 = 2 * state(1, 0);
|
||||
const Scalar _tmp3 = _tmp2 * state(3, 0);
|
||||
const Scalar _tmp4 = -_tmp1 + _tmp3;
|
||||
const Scalar _tmp5 =
|
||||
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 _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) * _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) = _tmp17;
|
||||
_innov_var(1, 0) = _tmp22;
|
||||
_innov_var(2, 0) = _tmp27;
|
||||
}
|
||||
|
||||
if (Kx != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _kx = (*Kx);
|
||||
|
||||
_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, 23, 1>& _ky = (*Ky);
|
||||
|
||||
_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, 23, 1>& _kz = (*Kz);
|
||||
|
||||
_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)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
|
|
@ -0,0 +1,77 @@
|
|||
// -----------------------------------------------------------------------------
|
||||
// This file was autogenerated by symforce from template:
|
||||
// function/FUNCTION.h.jinja
|
||||
// Do NOT modify by hand.
|
||||
// -----------------------------------------------------------------------------
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
namespace sym {
|
||||
|
||||
/**
|
||||
* This function was autogenerated from a symbolic function. Do not modify by hand.
|
||||
*
|
||||
* Symbolic function: compute_gravity_xyz_innov_var_and_hx
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* R: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Matrix31
|
||||
* Hx: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeGravityXyzInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Hx = nullptr) {
|
||||
// Total ops: 51
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (16)
|
||||
const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(0, 0), Scalar(2));
|
||||
const Scalar _tmp2 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp3 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp4 = _tmp0 + _tmp1 - _tmp2 - _tmp3;
|
||||
const Scalar _tmp5 = 2 * state(2, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(3, 0);
|
||||
const Scalar _tmp7 = 2 * state(1, 0);
|
||||
const Scalar _tmp8 = _tmp7 * state(0, 0);
|
||||
const Scalar _tmp9 = -_tmp6 - _tmp8;
|
||||
const Scalar _tmp10 = -_tmp0 - _tmp1 + _tmp2 + _tmp3;
|
||||
const Scalar _tmp11 = _tmp5 * state(0, 0);
|
||||
const Scalar _tmp12 = _tmp7 * state(3, 0);
|
||||
const Scalar _tmp13 = -_tmp11 + _tmp12;
|
||||
const Scalar _tmp14 = _tmp6 + _tmp8;
|
||||
const Scalar _tmp15 = _tmp11 - _tmp12;
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var(0, 0) = R + _tmp4 * (P(1, 1) * _tmp4 + P(2, 1) * _tmp9) +
|
||||
_tmp9 * (P(1, 2) * _tmp4 + P(2, 2) * _tmp9);
|
||||
_innov_var(1, 0) = R + _tmp10 * (P(0, 0) * _tmp10 + P(2, 0) * _tmp13) +
|
||||
_tmp13 * (P(0, 2) * _tmp10 + P(2, 2) * _tmp13);
|
||||
_innov_var(2, 0) = R + _tmp14 * (P(0, 0) * _tmp14 + P(1, 0) * _tmp15) +
|
||||
_tmp15 * (P(0, 1) * _tmp14 + P(1, 1) * _tmp15);
|
||||
}
|
||||
|
||||
if (Hx != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _hx = (*Hx);
|
||||
|
||||
_hx.setZero();
|
||||
|
||||
_hx(1, 0) = _tmp4;
|
||||
_hx(2, 0) = _tmp9;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
|
|
@ -0,0 +1,60 @@
|
|||
// -----------------------------------------------------------------------------
|
||||
// This file was autogenerated by symforce from template:
|
||||
// function/FUNCTION.h.jinja
|
||||
// Do NOT modify by hand.
|
||||
// -----------------------------------------------------------------------------
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
namespace sym {
|
||||
|
||||
/**
|
||||
* This function was autogenerated from a symbolic function. Do not modify by hand.
|
||||
*
|
||||
* Symbolic function: compute_gravity_y_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* R: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* Hy: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeGravityYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Hy = nullptr) {
|
||||
// Total ops: 22
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (2)
|
||||
const Scalar _tmp0 = -std::pow(state(0, 0), Scalar(2)) + std::pow(state(1, 0), Scalar(2)) +
|
||||
std::pow(state(2, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = -2 * state(0, 0) * state(2, 0) + 2 * state(1, 0) * state(3, 0);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R + _tmp0 * (P(0, 0) * _tmp0 + P(2, 0) * _tmp1) +
|
||||
_tmp1 * (P(0, 2) * _tmp0 + P(2, 2) * _tmp1);
|
||||
}
|
||||
|
||||
if (Hy != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _hy = (*Hy);
|
||||
|
||||
_hy.setZero();
|
||||
|
||||
_hy(0, 0) = _tmp0;
|
||||
_hy(2, 0) = _tmp1;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
|
|
@ -0,0 +1,61 @@
|
|||
// -----------------------------------------------------------------------------
|
||||
// This file was autogenerated by symforce from template:
|
||||
// function/FUNCTION.h.jinja
|
||||
// Do NOT modify by hand.
|
||||
// -----------------------------------------------------------------------------
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
namespace sym {
|
||||
|
||||
/**
|
||||
* This function was autogenerated from a symbolic function. Do not modify by hand.
|
||||
*
|
||||
* Symbolic function: compute_gravity_z_innov_var_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix23_23
|
||||
* R: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov_var: Scalar
|
||||
* Hz: Matrix23_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeGravityZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
|
||||
Scalar* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 23, 1>* const Hz = nullptr) {
|
||||
// Total ops: 18
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (4)
|
||||
const Scalar _tmp0 = 2 * state(2, 0);
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 = _tmp0 * state(3, 0) + _tmp1 * state(0, 0);
|
||||
const Scalar _tmp3 = _tmp0 * state(0, 0) - _tmp1 * state(3, 0);
|
||||
|
||||
// Output terms (2)
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R + _tmp2 * (P(0, 0) * _tmp2 + P(1, 0) * _tmp3) +
|
||||
_tmp3 * (P(0, 1) * _tmp2 + P(1, 1) * _tmp3);
|
||||
}
|
||||
|
||||
if (Hz != nullptr) {
|
||||
matrix::Matrix<Scalar, 23, 1>& _hz = (*Hz);
|
||||
|
||||
_hz.setZero();
|
||||
|
||||
_hz(0, 0) = _tmp2;
|
||||
_hz(1, 0) = _tmp3;
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
|
|
@ -826,10 +826,10 @@ PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.1f);
|
|||
* @group EKF2
|
||||
* @min 0.1
|
||||
* @max 10.0
|
||||
* @unit m/s^2
|
||||
* @unit g
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF2_GRAV_NOISE, 5.0f);
|
||||
PARAM_DEFINE_FLOAT(EKF2_GRAV_NOISE, 1.0f);
|
||||
|
||||
/**
|
||||
* Optical flow aiding
|
||||
|
|
|
|||
Loading…
Reference in New Issue