From 83079e0e11854ee4517c0dbd2a7989d7c2e806fd Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 4 Nov 2015 17:05:23 +1100 Subject: [PATCH] Inertial Nav EKF: add notes on use of generated code --- matlab/generated/Inertial Nav EKF/Notes.txt | 50 +++++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 matlab/generated/Inertial Nav EKF/Notes.txt diff --git a/matlab/generated/Inertial Nav EKF/Notes.txt b/matlab/generated/Inertial Nav EKF/Notes.txt new file mode 100644 index 0000000000..3518d28bd4 --- /dev/null +++ b/matlab/generated/Inertial Nav EKF/Notes.txt @@ -0,0 +1,50 @@ +NOTES FOR FUSION OF MEASUREMENTS USING AUTOCODE FRAGMENTS + +The auto-code for the fusion of the various measurements provides in most cases the observation Jacobian and Kalman gain matrix for that observation. +Where no Kalman gain is provided, it will need to be calculated using the usual K = P*transpose(H)/(H*P*transpose(H) + R) where: + +K = Kalman Gain matrix +H = observation partial derivative matrix (observaton Jacobian) +R = observation variance +(H*P*transpose(H) + R) is the innovation variance (always a scalar) + +When the observation is a vector, it is always assumed that the errors in the vectors are uncorelated to each other and the observations are fused +sequentially, so no matrix inversion is required. + +It is important that maximum useage of the sparsity in the H matrix be taken advantage of. If a sparse math library is available it could be used, +otherwise the matrix operations should need unrolled with inclusion of conditional statements to improve efficiency. Examples of this technique can +be found in the existing att_pos_ekf_estimator library + +It is important that the attitude error state vector (first three states) be zeroed prior to fusion of a new observation. This is becasue it is assumed +that the predicted quaternion attitude is corrected using the attitude error estimate each time an observation is fused. + +Measurement fusion steps: + +1) Calculate the innovation + +2) Calculate the observation partial derivative (Jacobian) vector. For direct state observations this has not been provided as it is trivial, +eg [0 0 0 1 0 .....] for a north velocity measurement + +3) Calculate the innovation variance and apply an innovatino consistency check to determine if he observation should be fused. BTW, if +(H*P*transpose(H) + R) < R, then the covariance matrix has become ill conditioned and should probably be reset. Certainly fusion of data +should not be performed if H*P*transpose(H) < 0 + +4) Calculate the Kalman gain vector + +5) Zero the first three states in the state vector + +6) Calculate and apply a correction to the states which is the product of the Kalman gain matrix and innovation + +7) The first three states represent the estimated angular misalignment vector. Rotate the predicted quaternion from the last INS calculation through +the reciprocal rotation to remove the error + +8) Update the covariance using the standard P = (I - K*H)*P operator, taking advatage of sparseness in the H and I matrices. This has been implemented +in the code as a P - KHP operation. Note, the use of the short rather thn Joseph or other mumerically more accurate form of the equation and use of single +precision operations means that numerical stability can be an issue, so symmetry and non-negative diagonal elements must be enforced after every +covariance update. + +NOTES FOR COVARIANCE PREDICTION + +Only expression for the upper diagonal is provided. The values will need to be copied across to the lower diagnal elements assuming symmetry +Process noise for time invariant states, eg Gyro bias, has not been included in the auto-code. The process noise variance for these states will +need to be added in a separate operation. \ No newline at end of file