mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
EKF2: centralized auto-generated state (#22183)
* ekf2_derivation: use single source of state definition The state is defined as an ordered dictionary of group elements and everything else is generated using that state definition * ekf2: generated state sample add const reference getter --------- Co-authored-by: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Co-authored-by: Daniel Agar <daniel@agar.ca>
This commit is contained in:
Mathieu Bresciani
GitHub
parent
2e1d5687f9
commit
05fd8c5976
@ -144,7 +144,7 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so
|
||||
|
||||
float innov = 0.f;
|
||||
float innov_var = 0.f;
|
||||
sym::ComputeAirspeedInnovAndInnovVar(getStateAtFusionHorizonAsVector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, &innov, &innov_var);
|
||||
sym::ComputeAirspeedInnovAndInnovVar(_state.vector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, &innov, &innov_var);
|
||||
|
||||
aid_src.observation = airspeed_sample.true_airspeed;
|
||||
aid_src.observation_variance = R;
|
||||
@ -195,7 +195,7 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour
|
||||
VectorState H; // Observation jacobian
|
||||
VectorState K; // Kalman gain vector
|
||||
|
||||
sym::ComputeAirspeedHAndK(getStateAtFusionHorizonAsVector(), P, innov_var, FLT_EPSILON, &H, &K);
|
||||
sym::ComputeAirspeedHAndK(_state.vector(), P, innov_var, FLT_EPSILON, &H, &K);
|
||||
|
||||
if (update_wind_only) {
|
||||
const Vector2f K_wind = K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0);
|
||||
|
||||
@ -269,17 +269,6 @@ struct systemFlagUpdate {
|
||||
bool gnd_effect{false};
|
||||
};
|
||||
|
||||
struct stateSample {
|
||||
Quatf quat_nominal{}; ///< quaternion defining the rotation from body to earth frame
|
||||
Vector3f vel{}; ///< NED velocity in earth frame in m/s
|
||||
Vector3f pos{}; ///< NED position in earth frame in m
|
||||
Vector3f gyro_bias{}; ///< gyro bias estimate in rad/s
|
||||
Vector3f accel_bias{}; ///< accel bias estimate in m/s^2
|
||||
Vector3f mag_I{}; ///< NED earth magnetic field in gauss
|
||||
Vector3f mag_B{}; ///< magnetometer bias estimate in body frame in gauss
|
||||
Vector2f wind_vel{}; ///< horizontal wind velocity in earth frame in m/s
|
||||
};
|
||||
|
||||
struct parameters {
|
||||
|
||||
int32_t filter_update_interval_us{10000}; ///< filter update interval in microseconds
|
||||
|
||||
@ -230,7 +230,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
|
||||
SquareMatrixState nextP;
|
||||
|
||||
// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
|
||||
sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P,
|
||||
sym::PredictCovariance(_state.vector(), P,
|
||||
imu_delayed.delta_vel, imu_delayed.delta_vel_dt, d_vel_var,
|
||||
imu_delayed.delta_ang, imu_delayed.delta_ang_dt, d_ang_var,
|
||||
&nextP);
|
||||
@ -542,7 +542,7 @@ 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(getStateAtFusionHorizonAsVector(), rot_var_ned, &q_cov);
|
||||
sym::RotVarNedToLowerTriangularQuatCov(_state.vector(), rot_var_ned, &q_cov);
|
||||
q_cov.copyLowerToUpperTriangle();
|
||||
resetStateCovariance<State::quat_nominal>(q_cov);
|
||||
}
|
||||
|
||||
@ -85,7 +85,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 = getStateAtFusionHorizonAsVector();
|
||||
const VectorState state_vector_prev = _state.vector();
|
||||
|
||||
Vector2f bcoef_inv;
|
||||
|
||||
|
||||
@ -304,7 +304,7 @@ public:
|
||||
void getGravityInnovRatio(float &grav_innov_ratio) const { grav_innov_ratio = Vector3f(_aid_src_gravity.test_ratio).max(); }
|
||||
|
||||
// get the state vector at the delayed time horizon
|
||||
matrix::Vector<float, State::size> getStateAtFusionHorizonAsVector() const;
|
||||
const matrix::Vector<float, State::size> &getStateAtFusionHorizonAsVector() const { return _state.vector(); }
|
||||
|
||||
// get the wind velocity in m/s
|
||||
const Vector2f &getWindVelocity() const { return _state.wind_vel; };
|
||||
@ -585,7 +585,7 @@ private:
|
||||
|
||||
Vector3f _ang_rate_delayed_raw{}; ///< uncorrected angular rate vector at fusion time horizon (rad/sec)
|
||||
|
||||
stateSample _state{}; ///< state struct of the ekf running at the delayed time horizon
|
||||
StateSample _state{}; ///< state struct of the ekf running at the delayed time horizon
|
||||
|
||||
bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised
|
||||
|
||||
|
||||
@ -369,21 +369,6 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
|
||||
}
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
// get the state vector at the delayed time horizon
|
||||
matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
|
||||
{
|
||||
matrix::Vector<float, 24> state;
|
||||
state.slice<State::quat_nominal.dof, 1>(State::quat_nominal.idx, 0) = _state.quat_nominal;
|
||||
state.slice<State::vel.dof, 1>(State::vel.idx, 0) = _state.vel;
|
||||
state.slice<State::pos.dof, 1>(State::pos.idx, 0) = _state.pos;
|
||||
state.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx, 0) = _state.gyro_bias;
|
||||
state.slice<State::accel_bias.dof, 1>(State::accel_bias.idx, 0) = _state.accel_bias;
|
||||
state.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) = _state.mag_I;
|
||||
state.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = _state.mag_B;
|
||||
state.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = _state.wind_vel;
|
||||
return state;
|
||||
}
|
||||
|
||||
bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const
|
||||
{
|
||||
origin_time = _pos_ref.getProjectionReferenceTimestamp();
|
||||
@ -907,7 +892,7 @@ void Ekf::updateVerticalDeadReckoningStatus()
|
||||
Vector3f Ekf::calcRotVecVariances() const
|
||||
{
|
||||
Vector3f rot_var;
|
||||
sym::QuatVarToRotVar(getStateAtFusionHorizonAsVector(), P, FLT_EPSILON, &rot_var);
|
||||
sym::QuatVarToRotVar(_state.vector(), P, FLT_EPSILON, &rot_var);
|
||||
return rot_var;
|
||||
}
|
||||
|
||||
|
||||
@ -65,7 +65,7 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample)
|
||||
|
||||
{
|
||||
VectorState H;
|
||||
sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
|
||||
sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, _gps_yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
|
||||
}
|
||||
|
||||
gnss_yaw.observation = measured_hdg;
|
||||
@ -97,7 +97,7 @@ void Ekf::fuseGpsYaw()
|
||||
|
||||
// Note: we recompute innov and innov_var because it doesn't cost much more than just computing H
|
||||
// making a separate function just for H uses more flash space without reducing CPU load significantly
|
||||
sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
|
||||
sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, _gps_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
|
||||
}
|
||||
|
||||
// check if the innovation variance calculation is badly conditioned
|
||||
|
||||
@ -62,7 +62,7 @@ void Ekf::controlGravityFusion(const imuSample &imu)
|
||||
Vector3f innovation_variance;
|
||||
VectorState Kx, Ky, Kz; // Kalman gain vectors
|
||||
sym::ComputeGravityInnovVarAndKAndH(
|
||||
getStateAtFusionHorizonAsVector(), P, measurement, measurement_var, FLT_EPSILON,
|
||||
_state.vector(), P, measurement, measurement_var, FLT_EPSILON,
|
||||
&innovation, &innovation_variance, &Kx, &Ky, &Kz);
|
||||
|
||||
// fill estimator aid source status
|
||||
|
||||
@ -63,7 +63,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 = getStateAtFusionHorizonAsVector();
|
||||
const VectorState state_vector = _state.vector();
|
||||
sym::ComputeMagInnovInnovVarAndHx(state_vector, P, mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H);
|
||||
|
||||
innov_var.copyTo(aid_src_mag.innovation_variance);
|
||||
@ -262,7 +262,7 @@ bool Ekf::fuseDeclination(float decl_sigma)
|
||||
float innovation_variance;
|
||||
|
||||
// TODO: review getMagDeclination() usage, use mag_I, _mag_declination_gps, or parameter?
|
||||
sym::ComputeMagDeclinationPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H);
|
||||
sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H);
|
||||
|
||||
const float innovation = wrap_pi(decl_pred - getMagDeclination());
|
||||
|
||||
|
||||
@ -77,11 +77,9 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
|
||||
aid_src.observation_variance[0] = R_LOS;
|
||||
aid_src.observation_variance[1] = R_LOS;
|
||||
|
||||
const VectorState state_vector = getStateAtFusionHorizonAsVector();
|
||||
|
||||
Vector2f innov_var;
|
||||
VectorState H;
|
||||
sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
|
||||
sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
|
||||
innov_var.copyTo(aid_src.innovation_variance);
|
||||
|
||||
// run the innovation consistency check and record result
|
||||
@ -96,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 = getStateAtFusionHorizonAsVector();
|
||||
const VectorState state_vector = _state.vector();
|
||||
|
||||
Vector2f innov_var;
|
||||
VectorState H;
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
#!/usr/bin/env python3
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Copyright (c) 2022 PX4 Development Team
|
||||
Copyright (c) 2022-2023 PX4 Development Team
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions
|
||||
are met:
|
||||
@ -37,82 +37,92 @@ import symforce
|
||||
symforce.set_epsilon_to_symbol()
|
||||
|
||||
import symforce.symbolic as sf
|
||||
from symforce import typing as T
|
||||
from symforce import ops
|
||||
from symforce.values import Values
|
||||
from derivation_utils import *
|
||||
|
||||
class State:
|
||||
qw = 0
|
||||
qx = 1
|
||||
qy = 2
|
||||
qz = 3
|
||||
vx = 4
|
||||
vy = 5
|
||||
vz = 6
|
||||
px = 7
|
||||
py = 8
|
||||
pz = 9
|
||||
gyro_bx = 10
|
||||
gyro_by = 11
|
||||
gyro_bz = 12
|
||||
accel_bx = 13
|
||||
accel_by = 14
|
||||
accel_bz = 15
|
||||
ix = 16
|
||||
iy = 17
|
||||
iz = 18
|
||||
ibx = 19
|
||||
iby = 20
|
||||
ibz = 21
|
||||
wx = 22
|
||||
wy = 23
|
||||
n_states = 24
|
||||
# The state vector is organized in an ordered dictionary
|
||||
State = Values(
|
||||
quat_nominal = sf.V4(),
|
||||
vel = sf.V3(),
|
||||
pos = sf.V3(),
|
||||
gyro_bias = sf.V3(),
|
||||
accel_bias = sf.V3(),
|
||||
mag_I = sf.V3(),
|
||||
mag_B = sf.V3(),
|
||||
wind_vel = sf.V2()
|
||||
)
|
||||
|
||||
class IdxDof():
|
||||
def __init__(self, idx, dof):
|
||||
self.idx = idx
|
||||
self.dof = dof
|
||||
|
||||
def BuildTangentStateIndex():
|
||||
# Build a dictionary that can be used to access elements of vectors
|
||||
# and matrices defined in the state tangent space (e.g.: P, K and H)
|
||||
tangent_state_index = {}
|
||||
idx = 0
|
||||
for key in State.keys_recursive():
|
||||
dof = State[key].tangent_dim()
|
||||
tangent_state_index[key] = IdxDof(idx, dof)
|
||||
idx += dof
|
||||
return tangent_state_index
|
||||
|
||||
tangent_idx = BuildTangentStateIndex()
|
||||
|
||||
class VState(sf.Matrix):
|
||||
SHAPE = (State.n_states, 1)
|
||||
SHAPE = (State.storage_dim(), 1)
|
||||
|
||||
class MState(sf.Matrix):
|
||||
SHAPE = (State.n_states, State.n_states)
|
||||
class VTangent(sf.Matrix):
|
||||
SHAPE = (State.tangent_dim(), 1)
|
||||
|
||||
def state_to_quat(state: VState) -> sf.Quaternion:
|
||||
return sf.Quaternion(sf.V3(state[State.qx], state[State.qy], state[State.qz]), state[State.qw])
|
||||
class MTangent(sf.Matrix):
|
||||
SHAPE = (State.tangent_dim(), State.tangent_dim())
|
||||
|
||||
def state_to_rot3(state: VState) -> sf.Rot3:
|
||||
return sf.Rot3(state_to_quat(state))
|
||||
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 predict_covariance(
|
||||
state: VState,
|
||||
P: MState,
|
||||
d_vel: sf.V3,
|
||||
d_vel_dt: sf.Scalar,
|
||||
d_vel_var: sf.V3,
|
||||
d_ang: sf.V3,
|
||||
d_ang_dt: sf.Scalar,
|
||||
d_ang_var: sf.Scalar
|
||||
):
|
||||
state: VState,
|
||||
P: MTangent,
|
||||
d_vel: sf.V3,
|
||||
d_vel_dt: sf.Scalar,
|
||||
d_vel_var: sf.V3,
|
||||
d_ang: sf.V3,
|
||||
d_ang_dt: sf.Scalar,
|
||||
d_ang_var: sf.Scalar
|
||||
) -> MTangent:
|
||||
|
||||
state = State.from_storage(state)
|
||||
g = sf.Symbol("g") # does not appear in the jacobians
|
||||
|
||||
accel_b = sf.V3(state[State.accel_bx], state[State.accel_by], state[State.accel_bz])
|
||||
d_vel_b = accel_b * d_vel_dt
|
||||
d_vel_b = state["accel_bias"] * d_vel_dt
|
||||
d_vel_true = d_vel - d_vel_b
|
||||
|
||||
gyro_b = sf.V3(state[State.gyro_bx], state[State.gyro_by], state[State.gyro_bz])
|
||||
d_ang_b = gyro_b * d_ang_dt
|
||||
d_ang_b = state["gyro_bias"] * d_ang_dt
|
||||
d_ang_true = d_ang - d_ang_b
|
||||
|
||||
q = state_to_quat(state)
|
||||
R_to_earth = sf.Rot3(q)
|
||||
v = sf.V3(state[State.vx], state[State.vy], state[State.vz])
|
||||
p = sf.V3(state[State.px], state[State.py], state[State.pz])
|
||||
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"]
|
||||
|
||||
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
|
||||
|
||||
# Predicted state vector at time t + dt
|
||||
state_new = VState.block_matrix([[sf.V4(q_new.w, q_new.x, q_new.y, q_new.z)], [v_new], [p_new], [sf.Matrix(state[State.gyro_bx:State.n_states])]])
|
||||
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,
|
||||
|
||||
# State propagation jacobian
|
||||
A = state_new.jacobian(state)
|
||||
G = state_new.jacobian(sf.V6.block_matrix([[d_vel], [d_ang]]))
|
||||
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)
|
||||
|
||||
# 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])
|
||||
@ -121,8 +131,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.n_states):
|
||||
for j in range(State.n_states):
|
||||
for index in range(state.storage_dim()):
|
||||
for j in range(state.storage_dim()):
|
||||
if index > j:
|
||||
P_new[index,j] = 0
|
||||
|
||||
@ -130,43 +140,48 @@ def predict_covariance(
|
||||
|
||||
def compute_airspeed_innov_and_innov_var(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
airspeed: sf.Scalar,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, sf.Scalar):
|
||||
|
||||
vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz])
|
||||
state = State.from_storage(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)
|
||||
H = sf.V1(airspeed_pred).jacobian(state, tangent_space=False)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov, innov_var)
|
||||
|
||||
def compute_airspeed_h_and_k(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
innov_var: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (VState, VState):
|
||||
) -> (VTangent, VTangent):
|
||||
|
||||
vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz])
|
||||
state = State.from_storage(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)
|
||||
H = sf.V1(airspeed_pred).jacobian(state, tangent_space=False)
|
||||
|
||||
K = P * H.T / sf.Max(innov_var, epsilon)
|
||||
|
||||
return (H.T, K)
|
||||
|
||||
def predict_sideslip(
|
||||
state: VState,
|
||||
state: State,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar):
|
||||
|
||||
vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz])
|
||||
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
|
||||
|
||||
# Small angle approximation of side slip model
|
||||
@ -177,166 +192,176 @@ def predict_sideslip(
|
||||
|
||||
def compute_sideslip_innov_and_innov_var(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, sf.Scalar, sf.Scalar):
|
||||
|
||||
state = State.from_storage(state)
|
||||
sideslip_pred = predict_sideslip(state, epsilon);
|
||||
|
||||
innov = sideslip_pred - 0.0
|
||||
|
||||
H = sf.V1(sideslip_pred).jacobian(state)
|
||||
H = sf.V1(sideslip_pred).jacobian(state, tangent_space=False)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov, innov_var)
|
||||
|
||||
def compute_sideslip_h_and_k(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
innov_var: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (VState, VState):
|
||||
) -> (VTangent, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
sideslip_pred = predict_sideslip(state, epsilon);
|
||||
|
||||
H = sf.V1(sideslip_pred).jacobian(state)
|
||||
H = sf.V1(sideslip_pred).jacobian(state, tangent_space=False)
|
||||
|
||||
K = P * H.T / sf.Max(innov_var, epsilon)
|
||||
|
||||
return (H.T, K)
|
||||
|
||||
def predict_mag_body(state) -> sf.V3:
|
||||
mag_field_earth = sf.V3(state[State.ix], state[State.iy], state[State.iz])
|
||||
mag_bias_body = sf.V3(state[State.ibx], state[State.iby], state[State.ibz])
|
||||
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
|
||||
return mag_body
|
||||
|
||||
def compute_mag_innov_innov_var_and_hx(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
meas: sf.V3,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.V3, sf.V3, VState):
|
||||
) -> (sf.V3, sf.V3, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
meas_pred = predict_mag_body(state);
|
||||
|
||||
innov = meas_pred - meas
|
||||
|
||||
innov_var = sf.V3()
|
||||
Hx = sf.V1(meas_pred[0]).jacobian(state)
|
||||
Hx = sf.V1(meas_pred[0]).jacobian(state, tangent_space=False)
|
||||
innov_var[0] = (Hx * P * Hx.T + R)[0,0]
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state)
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False)
|
||||
innov_var[1] = (Hy * P * Hy.T + R)[0,0]
|
||||
Hz = sf.V1(meas_pred[2]).jacobian(state)
|
||||
Hz = sf.V1(meas_pred[2]).jacobian(state, tangent_space=False)
|
||||
innov_var[2] = (Hz * P * Hz.T + R)[0,0]
|
||||
|
||||
return (innov, innov_var, Hx.T)
|
||||
|
||||
def compute_mag_y_innov_var_and_h(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VState):
|
||||
) -> (sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
meas_pred = predict_mag_body(state);
|
||||
|
||||
H = sf.V1(meas_pred[1]).jacobian(state)
|
||||
H = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
|
||||
def compute_mag_z_innov_var_and_h(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VState):
|
||||
) -> (sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
meas_pred = predict_mag_body(state);
|
||||
|
||||
H = sf.V1(meas_pred[2]).jacobian(state)
|
||||
H = sf.V1(meas_pred[2]).jacobian(state, tangent_space=False)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
|
||||
def compute_yaw_321_innov_var_and_h(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VState):
|
||||
) -> (sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
R_to_earth = state_to_rot3(state).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)
|
||||
H = sf.V1(meas_pred).jacobian(state, tangent_space=False)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
|
||||
def compute_yaw_321_innov_var_and_h_alternate(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VState):
|
||||
) -> (sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
R_to_earth = state_to_rot3(state).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)
|
||||
H = sf.V1(meas_pred).jacobian(state, tangent_space=False)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
|
||||
def compute_yaw_312_innov_var_and_h(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VState):
|
||||
) -> (sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
R_to_earth = state_to_rot3(state).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)
|
||||
H = sf.V1(meas_pred).jacobian(state, tangent_space=False)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
|
||||
def compute_yaw_312_innov_var_and_h_alternate(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VState):
|
||||
) -> (sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
R_to_earth = state_to_rot3(state).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)
|
||||
H = sf.V1(meas_pred).jacobian(state, tangent_space=False)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov_var, H.T)
|
||||
|
||||
def compute_mag_declination_pred_innov_var_and_h(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, sf.Scalar, VState):
|
||||
) -> (sf.Scalar, sf.Scalar, VTangent):
|
||||
|
||||
meas_pred = sf.atan2(state[State.iy], state[State.ix], epsilon=epsilon)
|
||||
state = State.from_storage(state)
|
||||
meas_pred = sf.atan2(state["mag_I"][1], state["mag_I"][0], epsilon=epsilon)
|
||||
|
||||
H = sf.V1(meas_pred).jacobian(state)
|
||||
H = sf.V1(meas_pred).jacobian(state, tangent_space=False)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (meas_pred, innov_var, H.T)
|
||||
@ -345,8 +370,7 @@ def predict_opt_flow(state, distance, epsilon):
|
||||
R_to_body = state_to_rot3(state).inverse()
|
||||
|
||||
# Calculate earth relative velocity in a non-rotating sensor frame
|
||||
v = sf.V3(state[State.vx], state[State.vy], state[State.vz])
|
||||
rel_vel_sensor = R_to_body * v
|
||||
rel_vel_sensor = R_to_body * state["vel"]
|
||||
|
||||
# Divide by range to get predicted angular LOS rates relative to X and Y
|
||||
# axes. Note these are rates in a non-rotating sensor frame
|
||||
@ -360,43 +384,46 @@ def predict_opt_flow(state, distance, epsilon):
|
||||
|
||||
def compute_flow_xy_innov_var_and_hx(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
distance: sf.Scalar,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.V2, VState):
|
||||
) -> (sf.V2, VTangent):
|
||||
state = State.from_storage(state)
|
||||
meas_pred = predict_opt_flow(state, distance, epsilon);
|
||||
|
||||
innov_var = sf.V2()
|
||||
Hx = sf.V1(meas_pred[0]).jacobian(state)
|
||||
Hx = sf.V1(meas_pred[0]).jacobian(state, tangent_space=False)
|
||||
innov_var[0] = (Hx * P * Hx.T + R)[0,0]
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state)
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False)
|
||||
innov_var[1] = (Hy * P * Hy.T + R)[0,0]
|
||||
|
||||
return (innov_var, Hx.T)
|
||||
|
||||
def compute_flow_y_innov_var_and_h(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
distance: sf.Scalar,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VState):
|
||||
) -> (sf.Scalar, VTangent):
|
||||
state = State.from_storage(state)
|
||||
meas_pred = predict_opt_flow(state, distance, epsilon);
|
||||
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state)
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False)
|
||||
innov_var = (Hy * P * Hy.T + R)[0,0]
|
||||
|
||||
return (innov_var, Hy.T)
|
||||
|
||||
def compute_gnss_yaw_pred_innov_var_and_h(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
antenna_yaw_offset: sf.Scalar,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, sf.Scalar, VState):
|
||||
) -> (sf.Scalar, sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
R_to_earth = state_to_rot3(state)
|
||||
|
||||
# define antenna vector in body frame
|
||||
@ -408,13 +435,13 @@ 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)
|
||||
H = sf.V1(meas_pred).jacobian(state, tangent_space=False)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (meas_pred, innov_var, H.T)
|
||||
|
||||
def predict_drag(
|
||||
state: VState,
|
||||
state: State,
|
||||
rho: sf.Scalar,
|
||||
cd: sf.Scalar,
|
||||
cm: sf.Scalar,
|
||||
@ -422,9 +449,8 @@ def predict_drag(
|
||||
) -> (sf.Scalar):
|
||||
R_to_body = state_to_rot3(state).inverse()
|
||||
|
||||
vel_rel = sf.V3(state[State.vx] - state[State.wx],
|
||||
state[State.vy] - state[State.wy],
|
||||
state[State.vz])
|
||||
wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0)
|
||||
vel_rel = state["vel"] - wind
|
||||
vel_rel_body = R_to_body * vel_rel
|
||||
vel_rel_body_xy = sf.V2(vel_rel_body[0], vel_rel_body[1])
|
||||
|
||||
@ -436,52 +462,53 @@ def predict_drag(
|
||||
|
||||
def compute_drag_x_innov_var_and_k(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
rho: sf.Scalar,
|
||||
cd: sf.Scalar,
|
||||
cm: sf.Scalar,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VState):
|
||||
) -> (sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
meas_pred = predict_drag(state, rho, cd, cm, epsilon)
|
||||
Hx = sf.V1(meas_pred[0]).jacobian(state)
|
||||
Hx = sf.V1(meas_pred[0]).jacobian(state, tangent_space=False)
|
||||
innov_var = (Hx * P * Hx.T + R)[0,0]
|
||||
Ktotal = P * Hx.T / sf.Max(innov_var, epsilon)
|
||||
K = VState()
|
||||
K[State.wx] = Ktotal[State.wx]
|
||||
K[State.wy] = Ktotal[State.wy]
|
||||
K = VTangent()
|
||||
K[tangent_idx["wind_vel"].idx: tangent_idx["wind_vel"].idx + tangent_idx["wind_vel"].dof] = Ktotal[tangent_idx["wind_vel"].idx: tangent_idx["wind_vel"].idx + tangent_idx["wind_vel"].dof]
|
||||
|
||||
return (innov_var, K)
|
||||
|
||||
def compute_drag_y_innov_var_and_k(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
rho: sf.Scalar,
|
||||
cd: sf.Scalar,
|
||||
cm: sf.Scalar,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, VState):
|
||||
) -> (sf.Scalar, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
meas_pred = predict_drag(state, rho, cd, cm, epsilon)
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state)
|
||||
Hy = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False)
|
||||
innov_var = (Hy * P * Hy.T + R)[0,0]
|
||||
Ktotal = P * Hy.T / sf.Max(innov_var, epsilon)
|
||||
K = VState()
|
||||
K[State.wx] = Ktotal[State.wx]
|
||||
K[State.wy] = Ktotal[State.wy]
|
||||
K = VTangent()
|
||||
K[tangent_idx["wind_vel"].idx: tangent_idx["wind_vel"].idx + tangent_idx["wind_vel"].dof] = Ktotal[tangent_idx["wind_vel"].idx: tangent_idx["wind_vel"].idx + tangent_idx["wind_vel"].dof]
|
||||
|
||||
return (innov_var, K)
|
||||
|
||||
def compute_gravity_innov_var_and_k_and_h(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
meas: sf.V3,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.V3, sf.V3, VState, VState, VState):
|
||||
) -> (sf.V3, sf.V3, VTangent, VTangent, VTangent):
|
||||
|
||||
state = State.from_storage(state)
|
||||
# get transform from earth to body frame
|
||||
R_to_body = state_to_rot3(state).inverse()
|
||||
|
||||
@ -497,7 +524,7 @@ 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)
|
||||
H = sf.V1(meas_pred[i]).jacobian(state, tangent_space=False)
|
||||
innov_var[i] = (H * P * H.T + R)[0,0]
|
||||
K[i] = P * H.T / innov_var[i]
|
||||
|
||||
@ -505,22 +532,24 @@ def compute_gravity_innov_var_and_k_and_h(
|
||||
|
||||
def quat_var_to_rot_var(
|
||||
state: VState,
|
||||
P: MState,
|
||||
P: MTangent,
|
||||
epsilon: sf.Scalar
|
||||
):
|
||||
J = sf.V3(state_to_rot3(state).to_tangent(epsilon=epsilon)).jacobian(state)
|
||||
) -> 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)
|
||||
q = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
|
||||
state = State.from_storage(state)
|
||||
q = state["quat_nominal"]
|
||||
attitude = state_to_rot3(state)
|
||||
J = q.jacobian(attitude)
|
||||
|
||||
@ -536,12 +565,11 @@ def rot_var_ned_to_lower_triangular_quat_cov(
|
||||
return q_var.lower_triangle()
|
||||
|
||||
print("Derive EKF2 equations...")
|
||||
generate_px4_function(predict_covariance, output_names=["P_new"])
|
||||
generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
|
||||
generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"])
|
||||
|
||||
generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"])
|
||||
generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"])
|
||||
generate_px4_function(predict_covariance, output_names=["P_new"])
|
||||
generate_px4_function(compute_mag_innov_innov_var_and_hx, output_names=["innov", "innov_var", "Hx"])
|
||||
generate_px4_function(compute_mag_y_innov_var_and_h, output_names=["innov_var", "H"])
|
||||
generate_px4_function(compute_mag_z_innov_var_and_h, output_names=["innov_var", "H"])
|
||||
@ -559,11 +587,4 @@ generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["inno
|
||||
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({"quat_nominal": sf.V4,
|
||||
"vel": sf.V3,
|
||||
"pos": sf.V3,
|
||||
"gyro_bias": sf.V3,
|
||||
"accel_bias": sf.V3,
|
||||
"mag_I": sf.V3,
|
||||
"mag_B": sf.V3,
|
||||
"wind_vel": sf.V2})
|
||||
generate_px4_state(State, tangent_idx)
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
#!/usr/bin/env python
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Copyright (c) 2022 PX4 Development Team
|
||||
Copyright (c) 2022-2023 PX4 Development Team
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions
|
||||
are met:
|
||||
@ -88,8 +88,63 @@ def generate_python_function(function_name, output_names):
|
||||
output_dir="generated",
|
||||
skip_directory_nesting=True)
|
||||
|
||||
def generate_px4_state(states):
|
||||
print("Generate EKF state definition")
|
||||
def build_state_struct(state, T="float"):
|
||||
out = "struct StateSample {\n"
|
||||
|
||||
def TypeFromLength(len):
|
||||
if len == 1:
|
||||
return f"{T}"
|
||||
elif len == 2:
|
||||
return f"matrix::Vector2<{T}>"
|
||||
elif len == 3:
|
||||
return f"matrix::Vector3<{T}>"
|
||||
elif len == 4:
|
||||
return f"matrix::Quaternion<{T}>"
|
||||
else:
|
||||
raise NotImplementedError
|
||||
|
||||
for key, val in state.items():
|
||||
out += f"\t{TypeFromLength(len(val))} {key}{{}};\n"
|
||||
|
||||
state_size = state.storage_dim()
|
||||
out += f"\n\tmatrix::Vector<{T}, {state_size}> Data() const {{\n" \
|
||||
+ f"\t\tmatrix::Vector<{T}, {state_size}> state;\n"
|
||||
|
||||
index = state.index()
|
||||
for key in index:
|
||||
out += f"\t\tstate.slice<{index[key].storage_dim}, 1>({index[key].offset}, 0) = {key};\n"
|
||||
|
||||
out += "\t\treturn state;\n"
|
||||
out += "\t};\n" # Data
|
||||
|
||||
# const ref vector access
|
||||
first_field = next(iter(state))
|
||||
|
||||
out += f"\n\tconst matrix::Vector<{T}, {state_size}>& vector() const {{\n" \
|
||||
+ f"\t\treturn *reinterpret_cast<matrix::Vector<{T}, {state_size}>*>(const_cast<float*>(reinterpret_cast<const {T}*>(&{first_field})));\n" \
|
||||
+ f"\t}};\n\n"
|
||||
|
||||
out += "};\n" # StateSample
|
||||
|
||||
out += f"static_assert(sizeof(matrix::Vector<{T}, {state_size}>) == sizeof(StateSample), \"state vector doesn't match StateSample size\");\n"
|
||||
|
||||
return out
|
||||
|
||||
def build_tangent_state_struct(state, tangent_state_index):
|
||||
out = "struct IdxDof { unsigned idx; unsigned dof; };\n"
|
||||
|
||||
out += "namespace State {\n"
|
||||
|
||||
start_index = 0
|
||||
for key in tangent_state_index.keys():
|
||||
out += f"\tstatic constexpr IdxDof {key}{{{tangent_state_index[key].idx}, {tangent_state_index[key].dof}}};\n"
|
||||
|
||||
out += f"\tstatic constexpr uint8_t size{{{state.tangent_dim()}}};\n"
|
||||
out += "};\n" # namespace State
|
||||
return out
|
||||
|
||||
def generate_px4_state(state, tangent_state_index):
|
||||
print("Generate EKF tangent state definition")
|
||||
filename = "state.h"
|
||||
f = open(f"./generated/{filename}", "w")
|
||||
header = ["// --------------------------------------------------\n",
|
||||
@ -97,21 +152,14 @@ def generate_px4_state(states):
|
||||
"// --------------------------------------------------\n",
|
||||
"\n#ifndef EKF_STATE_H",
|
||||
"\n#define EKF_STATE_H\n\n",
|
||||
"#include <matrix/math.hpp>\n\n",
|
||||
"namespace estimator\n{\n"]
|
||||
f.writelines(header)
|
||||
|
||||
f.write("struct IdxDof { unsigned idx; unsigned dof; };\n");
|
||||
f.write(build_state_struct(state))
|
||||
f.write("\n")
|
||||
f.write(build_tangent_state_struct(state, tangent_state_index))
|
||||
|
||||
f.write("namespace State {\n");
|
||||
|
||||
start_index = 0
|
||||
for (state_name, state_type) in states.items():
|
||||
tangent_dim = state_type.tangent_dim()
|
||||
f.write(f"\tstatic constexpr IdxDof {state_name}{{{start_index}, {tangent_dim}}};\n")
|
||||
start_index += tangent_dim
|
||||
|
||||
f.write(f"\tstatic constexpr uint8_t size{{{start_index}}};\n")
|
||||
f.write("};\n") # namespace State
|
||||
f.write("}\n") # namespace estimator
|
||||
f.write("#endif // !EKF_STATE_H\n")
|
||||
f.close()
|
||||
|
||||
@ -5,8 +5,40 @@
|
||||
#ifndef EKF_STATE_H
|
||||
#define EKF_STATE_H
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
namespace estimator
|
||||
{
|
||||
struct StateSample {
|
||||
matrix::Quaternion<float> quat_nominal{};
|
||||
matrix::Vector3<float> vel{};
|
||||
matrix::Vector3<float> pos{};
|
||||
matrix::Vector3<float> gyro_bias{};
|
||||
matrix::Vector3<float> accel_bias{};
|
||||
matrix::Vector3<float> mag_I{};
|
||||
matrix::Vector3<float> mag_B{};
|
||||
matrix::Vector2<float> wind_vel{};
|
||||
|
||||
matrix::Vector<float, 24> Data() const {
|
||||
matrix::Vector<float, 24> state;
|
||||
state.slice<4, 1>(0, 0) = quat_nominal;
|
||||
state.slice<3, 1>(4, 0) = vel;
|
||||
state.slice<3, 1>(7, 0) = pos;
|
||||
state.slice<3, 1>(10, 0) = gyro_bias;
|
||||
state.slice<3, 1>(13, 0) = accel_bias;
|
||||
state.slice<3, 1>(16, 0) = mag_I;
|
||||
state.slice<3, 1>(19, 0) = mag_B;
|
||||
state.slice<2, 1>(22, 0) = wind_vel;
|
||||
return state;
|
||||
};
|
||||
|
||||
const matrix::Vector<float, 24>& vector() const {
|
||||
return *reinterpret_cast<matrix::Vector<float, 24>*>(const_cast<float*>(reinterpret_cast<const float*>(&quat_nominal)));
|
||||
};
|
||||
|
||||
};
|
||||
static_assert(sizeof(matrix::Vector<float, 24>) == sizeof(StateSample), "state vector doesn't match StateSample size");
|
||||
|
||||
struct IdxDof { unsigned idx; unsigned dof; };
|
||||
namespace State {
|
||||
static constexpr IdxDof quat_nominal{0, 4};
|
||||
|
||||
@ -87,7 +87,7 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const
|
||||
|
||||
float innov = 0.f;
|
||||
float innov_var = 0.f;
|
||||
sym::ComputeSideslipInnovAndInnovVar(getStateAtFusionHorizonAsVector(), P, R, FLT_EPSILON, &innov, &innov_var);
|
||||
sym::ComputeSideslipInnovAndInnovVar(_state.vector(), P, R, FLT_EPSILON, &innov, &innov_var);
|
||||
|
||||
sideslip.observation = 0.f;
|
||||
sideslip.observation_variance = R;
|
||||
@ -136,7 +136,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
|
||||
VectorState H; // Observation jacobian
|
||||
VectorState K; // Kalman gain vector
|
||||
|
||||
sym::ComputeSideslipHAndK(getStateAtFusionHorizonAsVector(), P, sideslip.innovation_variance, FLT_EPSILON, &H, &K);
|
||||
sym::ComputeSideslipHAndK(_state.vector(), P, sideslip.innovation_variance, FLT_EPSILON, &H, &K);
|
||||
|
||||
if (update_wind_only) {
|
||||
const Vector2f K_wind = K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0);
|
||||
|
||||
@ -136,9 +136,9 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
|
||||
void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const
|
||||
{
|
||||
if (shouldUse321RotationSequence(_R_to_earth)) {
|
||||
sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
|
||||
sym::ComputeYaw321InnovVarAndH(_state.vector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
|
||||
|
||||
} else {
|
||||
sym::ComputeYaw312InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
|
||||
sym::ComputeYaw312InnovVarAndH(_state.vector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user