Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

ekf2: symforce - use builtin Rot3 and Quaternion operations #21359

Merged
merged 8 commits into from
Jun 5, 2023
Merged

ekf2: symforce - use builtin Rot3 and Quaternion operations #21359

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
3a23c53
ekf2: symforce - use builtin Rot3 and Quaternion operations
bresch Mar 22, 2023
871c2fd
DCM: use simplified conversion from unit quaternion
bresch Mar 29, 2023
96d4489
opt_flow: compare auto-generated Jacobian against autodiff
bresch Mar 27, 2023
d0f6603
mag_3d: compare auto-generated Jacobian against autodiff
bresch Mar 29, 2023
8f7b95e
sideslip: compare auto-generated Jacobian against autodiff
bresch Mar 29, 2023
4fbcbe2
yaw_fusion: compare auto-generated Jacobian against autodiff
bresch Mar 29, 2023
773f8dc
gnss yaw: compare auto-generated Jacobian against autodiff
bresch Mar 29, 2023
c521873
update change indicator
bresch Jun 2, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 3 additions & 4 deletions src/lib/matrix/matrix/Dcm.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -90,7 +90,6 @@ class Dcm : public SquareMatrix<Type, 3>
const Type b = q(1);
const Type c = q(2);
const Type d = q(3);
const Type aa = a * a;
const Type ab = a * b;
const Type ac = a * c;
const Type ad = a * d;
Expand All @@ -100,15 +99,15 @@ class Dcm : public SquareMatrix<Type, 3>
const Type cc = c * c;
const Type cd = c * d;
const Type dd = d * d;
dcm(0, 0) = aa + bb - cc - dd;
dcm(0, 0) = Type(1) - Type(2) * (cc + dd);
dcm(0, 1) = Type(2) * (bc - ad);
dcm(0, 2) = Type(2) * (ac + bd);
dcm(1, 0) = Type(2) * (bc + ad);
dcm(1, 1) = aa - bb + cc - dd;
dcm(1, 1) = Type(1) - Type(2) * (bb + dd);
dcm(1, 2) = Type(2) * (cd - ab);
dcm(2, 0) = Type(2) * (bd - ac);
dcm(2, 1) = Type(2) * (ab + cd);
dcm(2, 2) = aa - bb - cc + dd;
dcm(2, 2) = Type(1) - Type(2) * (bb + cc);
}

/**
Expand Down
46 changes: 20 additions & 26 deletions src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,12 @@ class VState(sf.Matrix):
class MState(sf.Matrix):
SHAPE = (State.n_states, State.n_states)

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])

def state_to_rot3(state: VState) -> sf.Rot3:
return sf.Rot3(state_to_quat(state))

def predict_covariance(
state: VState,
P: MState,
Expand All @@ -86,17 +92,17 @@ def predict_covariance(
d_ang_b = sf.V3(state[State.d_ang_bx], state[State.d_ang_by], state[State.d_ang_bz])
d_ang_true = d_ang - d_ang_b

q = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
R_to_earth = quat_to_rot_simplified(q)
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_new = quat_mult(q, sf.V4(1, 0.5 * d_ang_true[0], 0.5 * d_ang_true[1], 0.5 * d_ang_true[2]))
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) * dt
p_new = p + v * dt

# Predicted state vector at time t + dt
state_new = VState.block_matrix([[q_new], [v_new], [p_new], [sf.Matrix(state[State.d_ang_bx:State.n_states])]])
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.d_ang_bx:State.n_states])]])

# State propagation jacobian
A = state_new.jacobian(state)
Expand Down Expand Up @@ -155,8 +161,7 @@ def predict_sideslip(
) -> (sf.Scalar):

vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz])
q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
relative_wind_body = quat_to_rot(q_att).T * vel_rel
relative_wind_body = state_to_rot3(state).inverse() * vel_rel

# Small angle approximation of side slip model
# Protect division by zero using epsilon
Expand Down Expand Up @@ -196,11 +201,10 @@ def compute_sideslip_h_and_k(
return (H.T, K)

def predict_mag_body(state) -> sf.V3:
q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
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_body = quat_to_rot(q_att).T * mag_field_earth + mag_bias_body
mag_body = state_to_rot3(state).inverse() * mag_field_earth + mag_bias_body
return mag_body

def compute_mag_innov_innov_var_and_hx(
Expand Down Expand Up @@ -260,8 +264,7 @@ def compute_yaw_321_innov_var_and_h(
epsilon: sf.Scalar
) -> (sf.Scalar, VState):

q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
R_to_earth = quat_to_rot(q_att)
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)

Expand All @@ -277,8 +280,7 @@ def compute_yaw_321_innov_var_and_h_alternate(
epsilon: sf.Scalar
) -> (sf.Scalar, VState):

q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
R_to_earth = quat_to_rot(q_att)
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)

Expand All @@ -294,8 +296,7 @@ def compute_yaw_312_innov_var_and_h(
epsilon: sf.Scalar
) -> (sf.Scalar, VState):

q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
R_to_earth = quat_to_rot(q_att)
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)

Expand All @@ -311,8 +312,7 @@ def compute_yaw_312_innov_var_and_h_alternate(
epsilon: sf.Scalar
) -> (sf.Scalar, VState):

q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
R_to_earth = quat_to_rot(q_att)
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)

Expand All @@ -336,9 +336,7 @@ def compute_mag_declination_pred_innov_var_and_h(
return (meas_pred, innov_var, H.T)

def predict_opt_flow(state, distance, epsilon):
q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
R_to_earth = quat_to_rot(q_att)
R_to_body = R_to_earth.T
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])
Expand Down Expand Up @@ -393,8 +391,7 @@ def compute_gnss_yaw_pred_innov_var_and_h(
epsilon: sf.Scalar
) -> (sf.Scalar, sf.Scalar, VState):

q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
R_to_earth = quat_to_rot(q_att)
R_to_earth = state_to_rot3(state)

# define antenna vector in body frame
ant_vec_bf = sf.V3(sf.cos(antenna_yaw_offset), sf.sin(antenna_yaw_offset), 0)
Expand All @@ -417,9 +414,7 @@ def predict_drag(
cm: sf.Scalar,
epsilon: sf.Scalar
):
q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
R_to_earth = quat_to_rot(q_att)
R_to_body = R_to_earth.T
R_to_body = state_to_rot3(state).inverse()

vel_rel = sf.V3(state[State.vx] - state[State.wx],
state[State.vy] - state[State.wy],
Expand Down Expand Up @@ -481,8 +476,7 @@ def compute_gravity_innov_var_and_k_and_h(
) -> (sf.V3, sf.V3, VState, VState, VState):

# get transform from earth to body frame
q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
R_to_body = quat_to_rot(q_att).T
R_to_body = state_to_rot3(state).inverse()

# the innovation is the error between measured acceleration
# and predicted (body frame), assuming no body acceleration
Expand Down
38 changes: 0 additions & 38 deletions src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,44 +38,6 @@

import re

# q: quaternion describing rotation from frame 1 to frame 2
# returns a rotation matrix derived form q which describes the same
# rotation
def quat_to_rot(q):
q0 = q[0]
q1 = q[1]
q2 = q[2]
q3 = q[3]

Rot = sf.M33([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
[2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)],
[2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]])

return Rot

def quat_to_rot_simplified(q):
q0 = q[0]
q1 = q[1]
q2 = q[2]
q3 = q[3]

# Use the simplified formula for unit quaternion to rotation matrix
# as it produces a simpler and more stable EKF derivation given
# the additional constraint: q0^2 + q1^2 + q2^2 + q3^2 = 1
Rot = sf.Matrix([[1 - 2*q2**2 - 2*q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
[2*(q1*q2 + q0*q3), 1 - 2*q1**2 - 2*q3**2, 2*(q2*q3 - q0*q1)],
[2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), 1 - 2*q1**2 - 2*q2**2]])

return Rot

def quat_mult(p,q):
r = sf.Matrix([p[0] * q[0] - p[1] * q[1] - p[2] * q[2] - p[3] * q[3],
p[0] * q[1] + p[1] * q[0] + p[2] * q[3] - p[3] * q[2],
p[0] * q[2] - p[1] * q[3] + p[2] * q[0] + p[3] * q[1],
p[0] * q[3] + p[1] * q[2] - p[2] * q[1] + p[3] * q[0]])

return r

def sign_no_zero(x) -> sf.Scalar:
"""
Returns -1 if x is negative, 1 if x is positive, and 1 if x is zero
Expand Down
Loading