12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091 |
- function [...
- nextQuat, ... % quaternion state vector after fusion of measurements
- nextStates, ... % state vector after fusion of measurements
- nextP, ... % state covariance matrix after fusion of corrections
- innovation, ... % Declination innovation - rad
- varInnov] ... %
- = FuseMagnetometer( ...
- quat, ...
- states, ...
- P, ...
- magData, ...
- measDec, ...
- Tbn)
- q0 = quat(1);
- q1 = quat(2);
- q2 = quat(3);
- q3 = quat(4);
- magX = magData(1);
- magY = magData(2);
- magZ = magData(3);
- R_MAG = 0.1745^2;
- H = calcH_MAG(magX,magY,magZ,q0,q1,q2,q3);
- varInnov = (H*P*transpose(H) + R_MAG);
- Kfusion = (P*transpose(H))/varInnov;
- magMeasNED = Tbn*[magX;magY;magZ];
- predDec = atan2(magMeasNED(2),magMeasNED(1));
- innovation = predDec - measDec;
- if (innovation > pi)
- innovation = innovation - 2*pi;
- elseif (innovation < -pi)
- innovation = innovation + 2*pi;
- end
- if (innovation > 0.5)
- innovation = 0.5;
- elseif (innovation < -0.5)
- innovation = -0.5;
- end
- states(1:3) = 0;
- states = states - Kfusion * innovation;
- rotationMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2);
- if rotationMag<1e-6
- deltaQuat = single([1;0;0;0]);
- else
- deltaQuat = [cos(0.5*rotationMag); [states(1);states(2);states(3)]/rotationMag*sin(0.5*rotationMag)];
- end
- nextQuat = [quat(1)*deltaQuat(1)-transpose(quat(2:4))*deltaQuat(2:4); quat(1)*deltaQuat(2:4) + deltaQuat(1)*quat(2:4) + cross(quat(2:4),deltaQuat(2:4))];
- quatMag = sqrt(nextQuat(1)^2 + nextQuat(2)^2 + nextQuat(3)^2 + nextQuat(4)^2);
- if (quatMag > 1e-6)
- nextQuat = nextQuat / quatMag;
- end
- P = P - Kfusion*H*P;
- P = 0.5*(P + transpose(P));
- for i=1:9
- if P(i,i) < 0
- P(i,i) = 0;
- end
- end
- nextP = P;
- nextStates = states;
- end
|