From 70ece4df174af958a1f717e51ca2b18f09fd3c6a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 28 Jun 2017 15:22:06 +1000 Subject: [PATCH] matlab: Add references for coning and skulling correction method --- matlab/EKF_replay/Filter/PredictStates.m | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/matlab/EKF_replay/Filter/PredictStates.m b/matlab/EKF_replay/Filter/PredictStates.m index 3fb620ba7e..f1b06937f6 100644 --- a/matlab/EKF_replay/Filter/PredictStates.m +++ b/matlab/EKF_replay/Filter/PredictStates.m @@ -27,10 +27,14 @@ end delAng = delAng - states(11:13); delVel = delVel - states(14:16); -% Apply rotational and skulling corrections +% Correct delta velocity for rotation and skulling +% Derived from Eqn 25 of: +% "Computational Elements For Strapdown Systems" +% Savage, P.G. +% Strapdown Associates +% 2015, WBN-14010 correctedDelVel= delVel + ... - 0.5*cross(prevDelAng + delAng , prevDelVel + delVel) + 1/6*cross(prevDelAng + delAng , cross(prevDelAng + delAng , prevDelVel + delVel)) + ... % rotational correction - 1/12*(cross(prevDelAng , delVel) + cross(prevDelVel , delAng)); % sculling correction + 0.5*cross(prevDelAng + delAng , prevDelVel + delVel) + 1/6*cross(prevDelAng + delAng , cross(prevDelAng + delAng , prevDelVel + delVel)) + 1/12*(cross(prevDelAng , delVel) + cross(prevDelVel , delAng)); % Calculate earth delta angle spin vector delAngEarth_NED(1,1) = 0.000072921 * cos(latitude) * dt; @@ -38,6 +42,11 @@ delAngEarth_NED(2,1) = 0.0; delAngEarth_NED(3,1) = -0.000072921 * sin(latitude) * dt; % Apply corrections for coning errors and earth spin rate +% Coning correction from : +% "A new strapdown attitude algorithm", +% R. B. MILLER, +% Journal of Guidance, Control, and Dynamics +% July, Vol. 6, No. 4, pp. 287-291, Eqn 11 correctedDelAng = delAng - 1/12*cross(prevDelAng , delAng) - transpose(Tbn_prev)*delAngEarth_NED; % Save current measurements