├── EKF_AHRS.m ├── ESKF Attitude Algorithm.pdf ├── ESKF_AHRS.m ├── README.md ├── calH.m ├── calcFx.m ├── dataStructureDef.m ├── initializequat.m ├── plotFigure.m ├── propagateErrorStateAndCovar.m ├── propagateNominalState.m └── utils ├── axisAngleToRotMat.m ├── buildUpdateQuat.m ├── calChi2.m ├── crossMat.m ├── crossMatToVec.m ├── enforcePSD.m ├── omegaMat.m ├── quatInv.m ├── quatLeftComp.m ├── quatMult.m ├── quatRightComp.m ├── quatToRotMat.m ├── quaternProd.m ├── quatfromeuler.m ├── quattoeuler.m ├── readme.txt ├── removeCells.m └── renormalizeRotMat.m /EKF_AHRS.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yuzhou42/ESKF-Attitude-Estimation/9db3b4e97737c89c9357b8100482fb4f764796f9/EKF_AHRS.m -------------------------------------------------------------------------------- /ESKF Attitude Algorithm.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yuzhou42/ESKF-Attitude-Estimation/9db3b4e97737c89c9357b8100482fb4f764796f9/ESKF Attitude Algorithm.pdf -------------------------------------------------------------------------------- /ESKF_AHRS.m: -------------------------------------------------------------------------------- 1 | clear; 2 | close all; 3 | clc; 4 | addpath('utils'); 5 | tic 6 | %% ==========================Input Data======================== %% 7 | dataDir = '../datasets'; 8 | fileName = 'NAV_1'; 9 | Data = importdata(sprintf('%s/%s.mat',dataDir,fileName)); 10 | length = size(Data,1); 11 | dataStructureDef; 12 | %% ==========================Initial State======================== %% 13 | %get init euler angle and quaternion 14 | [qInit,roll(1),pitch(1),yaw(1)] = initializequat(Data); 15 | %Set up the noise parameters 16 | wn_var = 1e-5 * ones(1,3); % rot vel var 17 | wbn_var = 1e-9* ones(1,3); % gyro bias change var 18 | an_var = 1e-3 * ones(1,3); % acc var 19 | mn_var = 1e-4 * ones(1,3); % mag var 20 | noiseParams.Q = diag([wn_var, wbn_var]); 21 | noiseParams.R = diag([an_var, mn_var]); 22 | %set uo P initial value 23 | q_var_init = 1e-5 * ones(1,3); % init rot var 24 | wb_var_init = 1e-7 * ones(1,3); % init gyro bias var 25 | imuErrorStates{1}.P = diag([q_var_init, wb_var_init]); 26 | %Use ground truth for the first state 27 | imuNominalStates{1}.q = qInit; 28 | imuNominalStates{1}.wb = wbn_var'; 29 | %set up the reset value of error state 30 | detThetaReset = zeros(3,1); 31 | imuErrorStates{1}.det_theta = detThetaReset; 32 | imuErrorStates{1}.det_wb = detThetaReset; 33 | %MAIN LOOP 34 | for state_k = 1:length-1 35 | %% ==========================STATE PROPAGATION======================== %% 36 | %Propagate nominal state 37 | imuNominalStates{state_k+1} = propagateNominalState(imuNominalStates{state_k},measurements{state_k},measurements{state_k+1}); 38 | %Propagate error state and covariance 39 | imuErrorStates{state_k+1} = propagateErrorStateAndCovar(imuNominalStates{state_k}.wb,imuErrorStates{state_k}, measurements{state_k}, noiseParams); 40 | %% ==========================FILTER UPDATE======================== %% 41 | %Calculate H and detZ = y-h(det_x) 42 | [H,detZ] = calH(imuNominalStates{state_k+1}.q, measurements{state_k+1}); 43 | P = imuErrorStates{state_k+1}.P; 44 | % Calculate Kalman gain 45 | K = (P*H') / ( H*P*H' + noiseParams.R); 46 | % State correction 47 | det_x = K * detZ; 48 | imuErrorStates{state_k+1}.det_theta = det_x(1:3); 49 | imuErrorStates{state_k+1}.det_wb = det_x(4:6); 50 | % Covariance correction 51 | imuErrorStates{state_k+1}.P = P - K *(H*P*H'+ noiseParams.R)*K'; 52 | %% ==========================STATE CORRECTION======================== %% 53 | det_q = buildUpdateQuat(imuErrorStates{state_k+1}.det_theta); %it seems more safety to use this to update the euation q=[1 1/2*det_theta] 54 | imuNominalStates{state_k+1}.q = quatLeftComp(imuNominalStates{state_k+1}.q)*det_q; %joan sola_Quaternion kinematics for the error_state KF p24 55 | imuNominalStates{state_k+1}.q = imuNominalStates{state_k+1}.q /norm(imuNominalStates{state_k+1}.q ); 56 | imuNominalStates{state_k+1}.wb = imuNominalStates{state_k+1}.wb + imuErrorStates{state_k+1}.det_wb; 57 | [roll(state_k+1),pitch(state_k+1),yaw(state_k+1)] = quattoeuler(imuNominalStates{state_k+1}.q); %transform the quaternion to euler for plot 58 | %% ==========================ERROR STATE RESET======================== %% 59 | imuErrorStates{state_k+1}.det_theta = detThetaReset; 60 | imuErrorStates{state_k+1}.det_wb = detThetaReset; 61 | imuErrorStates{state_k+1}.P = eye(6)*imuErrorStates{state_k+1}.P*eye(6)'; 62 | % err_sigma(:,state_k) = sqrt(diag(imuErrorStates{state_k+1}.P)); 63 | end 64 | % err_sigma(:,length)=err_sigma(:,end); 65 | toc 66 | plotFigure 67 | 68 | 69 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ESKF-Attitude-Estimation 2 | Use Error-State KF algorithm to estimate attitude. 3 | The test data can be downloaded [here](https://drive.google.com/open?id=1opU5L2IdCs-FJpqHvzAba_W550uSlNzW). 4 | -------------------------------------------------------------------------------- /calH.m: -------------------------------------------------------------------------------- 1 | function [H,detZ] = calH(q,measurements_k) 2 | % Normalise magnetometer measurement 3 | if(norm(measurements_k.mag) == 0), return; end % 4 | measurements_k.mag = measurements_k.mag / norm(measurements_k.mag); % normalise magnitude,very important!!!! 5 | % Normalise accelerometer measurement 6 | if(norm(measurements_k.acc) == 0), return; end % handle NaN 7 | measurements_k.acc = measurements_k.acc / norm(measurements_k.acc); % normalise accelerometer ,very important!!!! 8 | % Reference direction of Earth's magnetic feild 9 | h = quaternProd(q, quaternProd([0; measurements_k.mag], quatInv(q))); 10 | b = [0 norm([h(2) h(3)]) 0 h(4)]; 11 | Ha = [2*q(3), -2*q(4), 2*q(1), -2*q(2) 12 | -2*q(2), -2*q(1), -2*q(4), -2*q(3) 13 | 0, 4*q(2), 4*q(3), 0]; 14 | Hm = [-2*b(4)*q(3), 2*b(4)*q(4), -4*b(2)*q(3)-2*b(4)*q(1), -4*b(2)*q(4)+2*b(4)*q(2) 15 | -2*b(2)*q(4)+2*b(4)*q(2), 2*b(2)*q(3)+2*b(4)*q(1), 2*b(2)*q(2)+2*b(4)*q(4), -2*b(2)*q(1)+2*b(4)*q(3) 16 | 2*b(2)*q(3), 2*b(2)*q(4)-4*b(4)*q(2), 2*b(2)*q(1)-4*b(4)*q(3), 2*b(2)*q(2)]; 17 | Hx = [Ha, zeros(3,3) 18 | Hm, zeros(3,3)]; 19 | %Hx = [Ha, zeros(3,3)]; 20 | Q_detTheta = [-q(2), -q(3), -q(4) 21 | q(1), -q(4), q(3) 22 | q(4), q(1), -q(2) 23 | -q(3), q(2), q(1)]; 24 | Xx = [0.5*Q_detTheta , zeros(4,3) 25 | zeros(3) , eye(3)]; 26 | H = Hx*Xx; 27 | 28 | detZ_a = [ 2*(q(2)*q(4) - q(1)*q(3)) + measurements_k.acc(1) 29 | 2*(q(1)*q(2) + q(3)*q(4)) + measurements_k.acc(2) 30 | 2*(0.5 - q(2)^2 - q(3)^2) + measurements_k.acc(3)]; 31 | % detZ = detZ_a; 32 | detZ_m =[((2*b(2)*(0.5 - q(3)^2 - q(4)^2) + 2*b(4)*(q(2)*q(4) - q(1)*q(3))) + measurements_k.mag(1)) 33 | ((2*b(2)*(q(2)*q(3) - q(1)*q(4)) + 2*b(4)*(q(1)*q(2) + q(3)*q(4))) + measurements_k.mag(2)) 34 | ((2*b(2)*(q(1)*q(3) + q(2)*q(4)) + 2*b(4)*(0.5 - q(2)^2 - q(3)^2)) + measurements_k.mag(3))]; 35 | detZ = [detZ_a;detZ_m]; 36 | end -------------------------------------------------------------------------------- /calcFx.m: -------------------------------------------------------------------------------- 1 | function Fx = calcFx(wb, measurements_k) 2 | % Multiplies the error state in the linearized continuous-time 3 | % error state model 4 | Fx = zeros(6,6); 5 | omegaHat = measurements_k.omega - wb; 6 | Fx(1:3,1:3) = axisAngleToRotMat(omegaHat*measurements_k.dt)'; 7 | Fx(1:3,4:6) = -eye(3)*measurements_k.dt; 8 | Fx(4:6,1:3) = zeros(3); 9 | Fx(4:6,4:6) = eye(3); 10 | end -------------------------------------------------------------------------------- /dataStructureDef.m: -------------------------------------------------------------------------------- 1 | time = zeros(length,1); 2 | roll = zeros(length,1); 3 | pitch = zeros(length,1); 4 | yaw = zeros(length,1); 5 | 6 | % IMU states as Structures indexed in a cell array 7 | %imuNominalStates 8 | % imuNominalStates{k}.q 4x1 Global to IMU rotation quaternion 9 | % imuNominalStates{k}.wb 3x1 Gyro bias 10 | imuNominalStates = cell(1,length); 11 | 12 | %imuErrorStates 13 | %imuErrorStates{k}.det_theta 3x1 det theta 14 | %imuErrorStates{k}.det_wb 3x1 det Gyro bias 15 | %imuErrorStates{k}.P 16 | imuErrorStates = cell(1,length); 17 | 18 | % Measurements as structures all indexed in a cell array 19 | %dT = [0, diff(t)]; 20 | %measurements 21 | %measurements{k}.acc 3x1 acc measurements 22 | %measurements{k}.omega 3x1 gyro measurements 23 | %measurements{k}.mag 3x1 mag measurements 24 | %measurements{k}.dt 25 | measurements = cell(1,length); 26 | %Ground Truth 27 | %groundTruthStates{k}.autitude 28 | for state_k = 1:length 29 | measurements{state_k}.dt = 0.02; % sampling times 50Hz 30 | measurements{state_k}.omega = Data(state_k,27:29)'; 31 | measurements{state_k}.acc = Data(state_k,9:11)'; 32 | measurements{state_k}.mag = Data(state_k,15:17)'; 33 | time(state_k)=state_k*0.02; 34 | end 35 | rad2deg = 180/pi; 36 | rollRef = Data(:,30)*rad2deg; 37 | pitchRef = Data(:,31)*rad2deg; 38 | yawRef = Data(:,32)*rad2deg; -------------------------------------------------------------------------------- /initializequat.m: -------------------------------------------------------------------------------- 1 | function [q,roll,pitch,yaw] = initializequat(Data) 2 | ax=Data(:,9); %acc data 3 | ay=Data(:,10); 4 | az=Data(:,11); 5 | mx=Data(:,15); %%mag data 6 | my=Data(:,16); 7 | mz=Data(:,17); 8 | rad2deg=180/pi; 9 | %% Process sensor data through algorithm 10 | Pitch0=asin(ax(1)/sqrt(ax(1)*ax(1)+ay(1)*ay(1)+az(1)*az(1))); 11 | Roll0=atan2(-ay(1),-az(1)); 12 | Yaw0=atan2(-my(1)*cos(Roll0)+mz(1)*sin(Roll0),mx(1)*cos(Pitch0)+my(1)*sin(Pitch0)*sin(Roll0)+mz(1)*sin(Pitch0)*cos(Roll0))-8.3*pi/180; 13 | [q0,q1,q2,q3]=quatfromeuler(Roll0,Pitch0,Yaw0); 14 | q = [q0;q1;q2;q3]; 15 | q = q/norm(q); 16 | pitch = Pitch0*rad2deg; 17 | roll = Roll0*rad2deg; 18 | yaw = Yaw0*rad2deg; 19 | end -------------------------------------------------------------------------------- /plotFigure.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yuzhou42/ESKF-Attitude-Estimation/9db3b4e97737c89c9357b8100482fb4f764796f9/plotFigure.m -------------------------------------------------------------------------------- /propagateErrorStateAndCovar.m: -------------------------------------------------------------------------------- 1 | function [errorState_prop] = propagateErrorStateAndCovar(wb,imuErrorStates_k, measurements_k, noiseParams) 2 | %Propagate State 3 | Fx = calcFx( wb, measurements_k); 4 | det_x = [imuErrorStates_k.det_theta;imuErrorStates_k.det_wb]; 5 | det_x_prop = Fx*det_x; %this propagate always be zero 6 | errorState_prop.det_theta = det_x_prop(1:3); 7 | errorState_prop.det_wb = det_x_prop(4:6); 8 | %Propagate Convar 9 | Qi = noiseParams.Q*measurements_k.dt; 10 | Fi = eye(6); 11 | errorState_prop.P = Fx * imuErrorStates_k.P * Fx' + Fi * Qi * Fi'; 12 | errorState_prop.P = enforcePSD(errorState_prop.P);%this is important,it removes the NAN problem successfully~ 13 | end -------------------------------------------------------------------------------- /propagateNominalState.m: -------------------------------------------------------------------------------- 1 | function nominalState_prop = propagateNominalState(imuNominalStates_k,measurement_k,measurement_k_1) 2 | % quaternion state 3 | det_theta = ((measurement_k.omega+measurement_k_1.omega)/2 - imuNominalStates_k.wb)* measurement_k.dt; 4 | %intergrade q with approximate form , this two equation have the same result. 5 | det_q = [1;0.5*det_theta]; 6 | nominalState_prop.q = quatLeftComp(imuNominalStates_k.q)*det_q; 7 | %nominalState_prop.q =imuNominalStates_k.q + 0.5 * omegaMat(det_theta) * imuNominalStates_k.q; 8 | 9 | % %intergrade q with close form 10 | % det_THETA = [0 -det_theta(1) -det_theta(2) -det_theta(3) 11 | % det_theta(1) 0 det_theta(3) -det_theta(2) 12 | % det_theta(2) -det_theta(3) 0 det_theta(1) 13 | % det_theta(3) det_theta(2) -det_theta(1) 0]; 14 | % norm_theta = norm(det_theta); 15 | % nominalState_prop.q = (cos(norm_theta/2)*eye(4)+sin(norm_theta/2)/norm_theta*det_THETA)*imuNominalStates_k.q; 16 | %Unit length quaternion 17 | nominalState_prop.q = nominalState_prop.q/norm( nominalState_prop.q); 18 | % Bias states 19 | nominalState_prop.wb = imuNominalStates_k.wb; 20 | end -------------------------------------------------------------------------------- /utils/axisAngleToRotMat.m: -------------------------------------------------------------------------------- 1 | function Psi = axisAngleToRotMat(psi) 2 | % 3 | % Converts an axis-angle rotation vector into a rotation matrix with 4 | % hamilton representation 5 | % 6 | % psi - axis-angle rotation vector 7 | %ref: joan sola 3D algebra for vision system in robotics p15 8 | theta = norm(psi); 9 | cp = cos(theta); 10 | sp = sin(theta); 11 | u = psi / norm(psi); 12 | uCross = crossMat(u); 13 | Psi = eye(3) + uCross*sp + uCross'* uCross*(1-cp); 14 | end -------------------------------------------------------------------------------- /utils/buildUpdateQuat.m: -------------------------------------------------------------------------------- 1 | function updateQuat = buildUpdateQuat(deltaTheta) 2 | % 3 | % Builds the update quaternion from the minimally parametrized update 4 | % See Indirect Kalman Filter for 3D Attitude Estimation (Roumeliotis) 5 | % 6 | 7 | deltaq = 0.5 * deltaTheta; 8 | 9 | checkNorm = deltaq' * deltaq; 10 | 11 | if checkNorm > 1 12 | updateQuat = [1; deltaq]; 13 | updateQuat = updateQuat / sqrt(1 + checkNorm); 14 | else 15 | updateQuat = [sqrt(1 - checkNorm);deltaq]; 16 | end 17 | 18 | updateQuat = updateQuat / norm(updateQuat); 19 | end -------------------------------------------------------------------------------- /utils/calChi2.m: -------------------------------------------------------------------------------- 1 | function chi2 = calChi2(angleDiff) 2 | angleDiff2 = angleDiff.^2; 3 | len = size(angleDiff,1); 4 | E = sum(angleDiff)/len; 5 | E2 = sum(angleDiff2)/len; 6 | var = E2-E^2; 7 | 8 | end -------------------------------------------------------------------------------- /utils/crossMat.m: -------------------------------------------------------------------------------- 1 | function vecCross = crossMat(vec) 2 | % 3 | % Computes the cross-product matrix of a 3x1 vector 4 | % 5 | if(size(vec,1) ~= 3 || size(vec,2) ~= 1) 6 | error('Input vector must be 3x1'); 7 | end 8 | 9 | vecCross = [ 0, -vec(3), vec(2); 10 | vec(3), 0, -vec(1); 11 | -vec(2), vec(1), 0 ]; 12 | end -------------------------------------------------------------------------------- /utils/crossMatToVec.m: -------------------------------------------------------------------------------- 1 | function vec = crossMatToVec(crossMat) 2 | % 3 | % Extracts a 3x1 vector from a cross-product matrix 4 | % 5 | % crossMat - 3x3 skew-symmetric matrix 6 | % 7 | vec = [crossMat(3,2); crossMat(1,3); crossMat(2,1)]; 8 | end -------------------------------------------------------------------------------- /utils/enforcePSD.m: -------------------------------------------------------------------------------- 1 | function matrixPSD = enforcePSD(matrix) 2 | if size(matrix,1) ~= size(matrix,2) 3 | error('Input matrix is not symmetric.'); 4 | else 5 | matrixPSD = matrix; 6 | for r = 1:size(matrixPSD,1) 7 | for c = 1:size(matrixPSD,2) 8 | if r == c 9 | matrixPSD(r,c) = abs(matrixPSD(r,c)); 10 | else 11 | offDiagElement = mean([matrixPSD(r,c),matrixPSD(c,r)]); 12 | % offDiagElement = matrixPSD(r,c); 13 | matrixPSD(c,r) = offDiagElement; 14 | matrixPSD(r,c) = offDiagElement; 15 | end 16 | end 17 | end 18 | end 19 | end -------------------------------------------------------------------------------- /utils/omegaMat.m: -------------------------------------------------------------------------------- 1 | function bigOmega = omegaMat(omega) 2 | % 3 | % Computes the Omega matrix of a 3x1 vector, omega 4 | %joan sola Quaternion kinematics for the error state KF p25 5 | if(size(omega,1) ~= 3 || size(omega,2) ~= 1) 6 | error('Input vector must be 3x1'); 7 | end 8 | 9 | bigOmega = [ 0 , -omega'; 10 | omega , -crossMat(omega) ]; 11 | end -------------------------------------------------------------------------------- /utils/quatInv.m: -------------------------------------------------------------------------------- 1 | function qInv = quatInv(quat) 2 | % 3 | % Computes the inverse (or conjugate) of a unit quaternion 4 | % using the {1,i,j,k} convention 5 | % 6 | if( size(quat,1) ~= 4 || size(quat,2) ~= 1 ) 7 | error('Input quaternion must be 4x1'); 8 | end 9 | 10 | % if( abs(norm(quat) - 1) > eps ) 11 | % error('Input quaternion must be unit-length'); 12 | % end 13 | 14 | qInv(1,1) = quat(1); 15 | qInv(2:4,1) = -quat(2:4); 16 | end -------------------------------------------------------------------------------- /utils/quatLeftComp.m: -------------------------------------------------------------------------------- 1 | function qLC = quatLeftComp(quat) 2 | % 3 | % Computes the right-hand compound operator form of a quaternion 4 | % using the {1,i,j,k} convention (q^+ in Tim's book)% 5 | % 6 | if( size(quat,1) ~= 4 || size(quat,2) ~= 1 ) 7 | error('Input quaternion must be 4x1'); 8 | end 9 | 10 | vector = quat(2:4); 11 | scalar = quat(1); 12 | 13 | qLC = [ scalar , -vector'; 14 | vector , scalar*eye(3) + crossMat(vector) ]; 15 | end -------------------------------------------------------------------------------- /utils/quatMult.m: -------------------------------------------------------------------------------- 1 | function quatProd = quatMult(quat1, quat2) 2 | % 3 | % Multiplies two quaternions using the {i,j,k,1} convention 4 | % 5 | if( size(quat1,1) ~= 4 || size(quat1,2) ~= 1 ... 6 | || size(quat2,1) ~= 4 || size(quat2,2) ~= 1 ) 7 | error('Input quaternions must be 4x1'); 8 | end 9 | 10 | quatProd = quatLeftComp(quat1) * quat2; 11 | end -------------------------------------------------------------------------------- /utils/quatRightComp.m: -------------------------------------------------------------------------------- 1 | function qRC = quatRightComp(quat) 2 | % 3 | % Computes the right-hand compound operator form of a quaternion 4 | % using the {1,i,j,k} convention joan sola quaternion kinematics for error 5 | % state kf p5 6 | % 7 | if( size(quat,1) ~= 4 || size(quat,2) ~= 1 ) 8 | error('Input quaternion must be 4x1'); 9 | end 10 | 11 | vector = quat(2:4); 12 | scalar = quat(1); 13 | 14 | qRC = [ scalar , -vector'; 15 | vector , scalar*eye(3) - crossMat(vector) ]; 16 | end -------------------------------------------------------------------------------- /utils/quatToRotMat.m: -------------------------------------------------------------------------------- 1 | function C = quatToRotMat(quat) 2 | % 3 | % Converts a quaternion into a 3x3 rotation matrix 4 | % using the {1,i,j,k} convention 5 | % 6 | if( size(quat,1) ~= 4 || size(quat,2) ~= 1 ) 7 | error('Input quaternion must be 4x1'); 8 | end 9 | 10 | if( abs(norm(quat) - 1) > eps ) 11 | if abs(norm(quat) - 1) > 0.1 12 | warning(sprintf('Input quaternion is not unit-length. norm(q) = %f. Re-normalizing.', norm(quat))); 13 | end 14 | quat = quat/norm(quat); 15 | end 16 | 17 | R = quatRightComp(quatInv(quat))' * quatLeftComp(quat); 18 | C = renormalizeRotMat( R(2:4,2:4) ); 19 | 20 | end -------------------------------------------------------------------------------- /utils/quaternProd.m: -------------------------------------------------------------------------------- 1 | function ab = quaternProd(a, b) 2 | %QUATERNPROD Calculates the quaternion product 3 | % 4 | % ab = quaternProd(a, b) 5 | % 6 | % Calculates the quaternion product of quaternion a and b. 7 | % 8 | % For more information see: 9 | % http://www.x-io.co.uk/node/8#quaternions 10 | % 11 | % Date Author Notes 12 | % 27/09/2011 SOH Madgwick Initial release 13 | 14 | ab(1) = a(1).*b(1)-a(2).*b(2)-a(3).*b(3)-a(4).*b(4); 15 | ab(2) = a(1).*b(2)+a(2).*b(1)+a(3).*b(4)-a(4).*b(3); 16 | ab(3) = a(1).*b(3)-a(2).*b(4)+a(3).*b(1)+a(4).*b(2); 17 | ab(4) = a(1).*b(4)+a(2).*b(3)-a(3).*b(2)+a(4).*b(1); 18 | end 19 | 20 | -------------------------------------------------------------------------------- /utils/quatfromeuler.m: -------------------------------------------------------------------------------- 1 | function [w,x,y,z]=quatfromeuler( ea_fRoll,ea_fPitch,ea_fYaw) 2 | 3 | fCosHRoll = cos(ea_fRoll * 0.5); 4 | fSinHRoll = sin(ea_fRoll * 0.5); 5 | fCosHPitch = cos(ea_fPitch * 0.5); 6 | fSinHPitch = sin(ea_fPitch * 0.5); 7 | fCosHYaw = cos(ea_fYaw * 0.5); 8 | fSinHYaw = sin(ea_fYaw * 0.5); 9 | 10 | w = fCosHRoll*fCosHPitch*fCosHYaw+fSinHRoll*fSinHPitch*fSinHYaw; 11 | x = fCosHPitch * fSinHRoll * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw; 12 | y = fCosHRoll * fCosHYaw * fSinHPitch + fSinHRoll * fCosHPitch * fSinHYaw; 13 | z = fCosHRoll * fCosHPitch * fSinHYaw - fCosHYaw * fSinHPitch * fSinHRoll; 14 | -------------------------------------------------------------------------------- /utils/quattoeuler.m: -------------------------------------------------------------------------------- 1 | function [roll,pitch,yaw] = quattoeuler(q) 2 | rad2deg=180/pi; 3 | T=[ 1 - 2 * (q(4) *q(4) + q(3) * q(3)) 2 * (q(2) * q(3) +q(1) * q(4)) 2 * (q(2) * q(4)-q(1) * q(3)); 4 | 2 * (q(2) * q(3)-q(1) * q(4)) 1 - 2 * (q(4) *q(4) + q(2) * q(2)) 2 * (q(3) * q(4)+q(1) * q(2)); 5 | 2 * (q(2) * q(4) +q(1) * q(3)) 2 * (q(3) * q(4)-q(1) * q(2)) 1 - 2 * (q(2) *q(2) + q(3) * q(3))];%cnb 6 | roll = atan2(T(2,3),T(3,3))*rad2deg; 7 | pitch = asin(-T(1,3))*rad2deg; 8 | yaw = atan2(T(1,2),T(1,1))*rad2deg-8.3; 9 | end -------------------------------------------------------------------------------- /utils/readme.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yuzhou42/ESKF-Attitude-Estimation/9db3b4e97737c89c9357b8100482fb4f764796f9/utils/readme.txt -------------------------------------------------------------------------------- /utils/removeCells.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yuzhou42/ESKF-Attitude-Estimation/9db3b4e97737c89c9357b8100482fb4f764796f9/utils/removeCells.m -------------------------------------------------------------------------------- /utils/renormalizeRotMat.m: -------------------------------------------------------------------------------- 1 | function C_unitary = renormalizeRotMat(C) 2 | % Enforce det(C) = 1 by finding the nearest orthogonal matrix 3 | [U,S,V] = svd(C); 4 | C_unitary = U*eye(size(S,1))*V'; 5 | end --------------------------------------------------------------------------------