dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

matlab: Add references for coning and skulling correction method

This commit is contained in:
Paul Riseborough 2017-06-28 15:22:06 +10:00
parent 8ccc45b26b
commit 70ece4df17
1 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
matlab/EKF_replay/Filter/PredictStates.m
View File

@ -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