123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161 |
- clear all;
- syms dax day daz real
- syms dvx dvy dvz real
- syms q0 q1 q2 q3 real
- syms vn ve vd real
- syms dax_b day_b daz_b real
- syms dvx_b dvy_b dvz_b real
- syms dt real
- syms gravity real
- syms daxNoise dayNoise dazNoise dvxNoise dvyNoise dvzNoise real;
- syms vwn vwe real;
- syms magX magY magZ real;
- syms magN magE magD real;
- syms R_VN R_VE R_VD real
- syms R_MAG real
- syms rotErr1 rotErr2 rotErr3 real;
- dAngMeas = [dax; day; daz];
- dVelMeas = [dvx; dvy; dvz];
- dAngBias = [dax_b; day_b; daz_b];
- estQuat = [q0;q1;q2;q3];
- errRotVec = [rotErr1;rotErr2;rotErr3];
- errQuat = [1;0.5*errRotVec];
- truthQuat = QuatMult(estQuat, errQuat);
- Tbn = Quat2Tbn(truthQuat);
- dAngTruth = dAngMeas - dAngBias - [daxNoise;dayNoise;dazNoise];
- dVelTruth = dVelMeas - [dvxNoise;dvyNoise;dvzNoise];
- deltaQuat = [1;
- 0.5*dAngTruth(1);
- 0.5*dAngTruth(2);
- 0.5*dAngTruth(3);
- ];
- truthQuatNew = QuatMult(truthQuat,deltaQuat);
- errQuatNew = QuatDivide(truthQuatNew,estQuat);
- errRotNew = 2 * [errQuatNew(2);errQuatNew(3);errQuatNew(4)];
- vNew = [vn;ve;vd] + [0;0;gravity]*dt + Tbn*dVelTruth;
- dabNew = [dax_b; day_b; daz_b];
- stateVector = [errRotVec;vn;ve;vd;dAngBias];
- nStates=numel(stateVector);
- distVector = [daxNoise;dayNoise;dazNoise;dvxNoise;dvyNoise;dvzNoise];
- G = jacobian([errRotNew;vNew;dabNew], distVector);
- G = subs(G, {'rotErr1', 'rotErr2', 'rotErr3'}, {0,0,0});
- distMatrix = diag(distVector);
- Q = G*distMatrix*transpose(G);
- f = matlabFunction(Q,'file','calcQ.m');
- vNew = subs(vNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0});
- errRotNew = subs(errRotNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0});
- F = jacobian([errRotNew;vNew;dabNew], stateVector);
- F = subs(F, {'rotErr1', 'rotErr2', 'rotErr3'}, {0,0,0});
- f = matlabFunction(F,'file','calcF.m');
- magMeasNED = Tbn*[magX;magY;magZ];
- angMeas = tan(magMeasNED(2)/magMeasNED(1));
- H_MAG = jacobian(angMeas,stateVector);
- H_MAG = subs(H_MAG, {'rotErr1', 'rotErr2', 'rotErr3'}, {0,0,0});
- f = matlabFunction(H_MAG,'file','calcH_MAG.m');
|