12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970 |
- function [quat, states, Tbn, correctedDelAng, correctedDelVel] = PredictStates( ...
- quat, ... % previous quaternion states 4x1
- states, ... % previous states (3x1 rotation error, 3x1 velocity, 3x1 gyro bias)
- angRate, ...
- accel, ...
- dt)
- persistent prevAngRate;
- if isempty(prevAngRate)
- prevAngRate = angRate;
- end
- persistent prevAccel;
- if isempty(prevAccel)
- prevAccel = accel;
- end
- persistent prevDelAng;
- if isempty(prevDelAng)
- prevDelAng = prevAngRate*dt;
- end
- persistent prevDelVel;
- if isempty(prevDelVel)
- prevDelVel = prevAccel*dt;
- end
- dAng = 0.5*(angRate + prevAngRate)*dt;
- dVel = 0.5*(accel + prevAccel )*dt;
- prevAngRate = angRate;
- prevAccel = accel;
- dAng = dAng - states(7:9);
- correctedDelVel= dVel + ...
- 0.5*cross(prevDelAng + dAng , prevDelVel + dVel) + 1/6*cross(prevDelAng + dAng , cross(prevDelAng + dAng , prevDelVel + dVel)) + ...
- 1/12*(cross(prevDelAng , dVel) + cross(prevDelVel , dAng));
- correctedDelAng = dAng - 1/12*cross(prevDelAng , dAng);
- prevDelAng = dAng;
- prevDelVel = dVel;
- deltaQuat = RotToQuat(correctedDelAng);
- quat = QuatMult(quat,deltaQuat);
- quat = NormQuat(quat);
- Tbn = Quat2Tbn(quat);
-
- delVelNav = Tbn * correctedDelVel + [0;0;9.807]*dt;
- states(4:6) = states(4:6) + delVelNav(1:3);
- end
|