123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113 |
- 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
- dAngMeas = [dax; day; daz];
- dVelMeas = [dvx; dvy; dvz];
- dAngBias = [dax_b; day_b; daz_b];
- quat = [q0;q1;q2;q3];
- Tbn = Quat2Tbn(quat);
- dAngTruth = dAngMeas - dAngBias;
- dVelTruth = dVelMeas;
- deltaQuat = [1;
- 0.5*dAngTruth(1);
- 0.5*dAngTruth(2);
- 0.5*dAngTruth(3);
- ];
- quatNew = QuatMult(quat,deltaQuat);
- vNew = [vn;ve;vd] + [0;0;gravity]*dt + Tbn*dVelTruth;
- dabNew = [dax_b; day_b; daz_b];
- stateVector = [quat;vn;ve;vd;dAngBias];
- nStates=numel(stateVector);
- distVector = [dAngMeas;dVelMeas];
- G = jacobian([quatNew;vNew;dabNew], distVector);
- f = matlabFunction(G,'file','calcG.m');
- imuNoise = diag([daxNoise dayNoise dazNoise dvxNoise dvyNoise dvzNoise]);
- Q = G*imuNoise*transpose(G);
- f = matlabFunction(Q,'file','calcQ.m');
- F = jacobian([quatNew;vNew;dabNew], stateVector);
- f = matlabFunction(F,'file','calcF.m');
- magMeasNED = Tbn*[magX;magY;magZ];
- angMeas = tan(magMeasNED(2)/magMeasNED(1));
- H_MAG = jacobian(angMeas,stateVector);
- f = matlabFunction(H_MAG,'file','calcH_MAG.m');
|