mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF3: document provenance of tilt error variance equations
This is also from the older version of the generator. Note that as documented, some of the equations have been removed and rearranged slightly as it is assumed their terms are zero. Additionally, the result is taken as the sum of the diagonal entries of the matrix.
This commit is contained in:
parent
34fba4dfd7
commit
f9fa2565c4
|
@ -2122,7 +2122,7 @@ void NavEKF3_core::calcTiltErrorVariance()
|
|||
const ftype &q2 = stateStruct.quat[2];
|
||||
const ftype &q3 = stateStruct.quat[3];
|
||||
|
||||
// equations generated by quaternion_error_propagation(): in AP_NavEKF3/derivation/main.py
|
||||
// equations generated by quaternion_error_propagation(): in derivation/generate_2.py
|
||||
// only diagonals have been used
|
||||
// dq0 ... dq3 terms have been zeroed
|
||||
const ftype PS1 = q0*q1 + q2*q3;
|
||||
|
|
|
@ -678,8 +678,8 @@ def generate_code():
|
|||
|
||||
|
||||
# derive autocode for other methods
|
||||
print('Computing tilt error covariance matrix ...')
|
||||
quaternion_error_propagation()
|
||||
# print('Computing tilt error covariance matrix ...')
|
||||
# quaternion_error_propagation()
|
||||
# print('Generating heading observation code ...')
|
||||
# yaw_observation(P,state,R_to_earth)
|
||||
print('Generating gps heading observation code ...')
|
||||
|
|
|
@ -665,8 +665,8 @@ def generate_code():
|
|||
|
||||
|
||||
# derive autocode for other methods
|
||||
# print('Computing tilt error covariance matrix ...')
|
||||
# quaternion_error_propagation()
|
||||
print('Computing tilt error covariance matrix ...')
|
||||
quaternion_error_propagation()
|
||||
print('Generating heading observation code ...')
|
||||
yaw_observation(P,state,R_to_earth)
|
||||
# print('Generating gps heading observation code ...')
|
||||
|
|
Loading…
Reference in New Issue