├── @DCM_IMU └── DCM_IMU.m ├── IMUcalibIteration.m ├── MIT-LICENSE.txt ├── README.md ├── allData.mat ├── c ├── DCM_IMU │ ├── DCM_IMU_C.cpp │ ├── DCM_IMU_C.h │ └── matlabmex.cpp ├── DCM_IMU_uc │ ├── DCM_IMU_uC.cpp │ ├── DCM_IMU_uC.h │ └── matlabmex.cpp ├── MadgwickAHRS │ └── matlabmex.cpp ├── MahonyAHRS │ └── matlabmex.cpp └── compile.m ├── fitRegressionLine.m ├── imusWithKukaCalibration.m ├── legendLocation.m ├── plotIMUsWithKuka.m ├── plotIMUsWithKuka_biasTest.m ├── rootMeanSquaredErrors.m ├── un_modulo.m └── yawpitchroll.m /@DCM_IMU/DCM_IMU.m: -------------------------------------------------------------------------------- 1 | %============================================================================ 2 | % Copyright (C) 2015, Heikki Hyyti 3 | % 4 | % Permission is hereby granted, free of charge, to any person obtaining a 5 | % copy of this software and associated documentation files (the "Software"), 6 | % to deal in the Software without restriction, including without limitation 7 | % the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | % and/or sell copies of the Software, and to permit persons to whom the 9 | % Software is furnished to do so, subject to the following conditions: 10 | % 11 | % The above copyright notice and this permission notice shall be included in 12 | % all copies or substantial portions of the Software. 13 | % 14 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | % THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | % FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | % DEALINGS IN THE SOFTWARE. 21 | %============================================================================ 22 | 23 | classdef DCM_IMU < handle 24 | % DCM_IMU Implementation of Hyyti's IMU algorithm 25 | % 26 | % If you use the algorithm in any scientific context, please cite: 27 | % Heikki Hyyti and Arto Visala, "A DCM Based Attitude Estimation Algorithm for Low-Cost MEMS IMUs," 28 | % International Journal of Navigation and Observation, vol. 2015, Article ID 503814, 18 pages, 2015. 29 | % http://dx.doi.org/10.1155/2015/503814 30 | % 31 | % Date Author Notes 32 | % 1/12/2015 Heikki Hyyti Initial release 33 | 34 | %% Public properties 35 | properties (Access = public) 36 | g0 = 9.8189; % gravitation around Helsinki, Finland (change according to your area) 37 | state = [0 0 1 0 0 0]'; % States are lowest row of rotation matrix and gyroscope x y and z biases 38 | % (C_31, C_32, C_33, w_b1, w_b2, w_b3) 39 | q_dcm2 = 0.1^2; % estimated variance of dcm states (gyro variance per second) 40 | q_gyro_bias2 = 0.0001^2; % very small number to make bias change slowly 41 | r_acc2 = 0.5^2; % variance of calibrated accelerometer (g-component) 42 | r_a2 = 10^2; % large variance for some unknown acceleration (acc = a + g) 43 | q_dcm2_init = 1^2; % initial variance of dcm states (for attitude estimation) 44 | q_gyro_bias2_init = 0.1^2; % initial variance of bias states (for bias estimator) 45 | a = zeros(3,1); % estimated non-gravitational accelerations 46 | yaw = 0; % Yaw angle around z axis (in ZYX convention) 47 | pitch = 0; % Pitch angle around y axis 48 | roll = 0; % Roll angle around x axis 49 | P = []; % estimate covariance (these are initialized in constructor below) 50 | H = []; % observation model (static) 51 | Q = []; % proces noise covariance (static part) 52 | first_row = [1 0 0]'; % first row of of the rotation matrix (for yaw angle estimate) 53 | end 54 | 55 | %% Public methods 56 | methods (Access = public) 57 | function obj = DCM_IMU(varargin) 58 | updateP = true; 59 | for i = 1:2:nargin 60 | if strcmp(varargin{i}, 'Gravity'), obj.g0 = varargin{i+1}; 61 | elseif strcmp(varargin{i}, 'State'), obj.state = varargin{i+1}; 62 | elseif strcmp(varargin{i}, 'Covariance'), obj.P = varargin{i+1}; updateP = false; 63 | elseif strcmp(varargin{i}, 'DCMVariance'), obj.q_dcm2 = varargin{i+1}; 64 | elseif strcmp(varargin{i}, 'BiasVariance'), obj.q_gyro_bias2 = varargin{i+1}; 65 | elseif strcmp(varargin{i}, 'InitialDCMVariance'), obj.q_dcm2_init = varargin{i+1}; 66 | elseif strcmp(varargin{i}, 'InitialBiasVariance'), obj.q_gyro_bias2_init = varargin{i+1}; 67 | elseif strcmp(varargin{i}, 'MeasurementVariance'), obj.r_acc2 = varargin{i+1}; 68 | elseif strcmp(varargin{i}, 'MeasurementVarianceVariableGain'), obj.r_a2 = varargin{i+1}; 69 | else error('Invalid argument'); 70 | end 71 | end; 72 | 73 | if (updateP), obj.P = [obj.q_dcm2_init*eye(3), zeros(3,3); zeros(3,3), obj.q_gyro_bias2_init*eye(3)]; end; 74 | obj.H = [eye(3)*obj.g0, zeros(3,3)]; 75 | obj.Q = [obj.q_dcm2*eye(3), zeros(3,3); zeros(3,3) obj.q_gyro_bias2*eye(3)]; 76 | end 77 | function obj = UpdateIMU(obj, Gyroscope, Accelerometer, SamplePeriod) 78 | x = obj.state; 79 | x_last = x; 80 | Q_ = SamplePeriod^2 * obj.Q; %Process noise covariance with time dependent noise 81 | 82 | % control input (angular velocities from gyroscopes) 83 | if (size(Gyroscope,1) == 3), u = Gyroscope; 84 | else u = Gyroscope'; 85 | end 86 | 87 | % "rotation operators" 88 | C3X = [0 -x(3) x(2); x(3) 0 -x(1); -x(2) x(1) 0]; 89 | UX = [0 -(u(3)-x(6)) u(2)-x(5); 90 | u(3)-x(6) 0 -(u(1)-x(4)); 91 | -(u(2)-x(5)) u(1)-x(4) 0]; 92 | 93 | % Model generation 94 | A = [zeros(3,3) -SamplePeriod*C3X; zeros(3,6)]; 95 | B = [SamplePeriod*C3X; zeros(3,3)]; 96 | F = eye(6) + [-SamplePeriod*UX, -SamplePeriod*C3X; zeros(3,6)]; 97 | 98 | % Kalman a priori prediction 99 | x_predict = x + A*x + B*u; 100 | P_predict = F * obj.P * F' + Q_; 101 | 102 | % measurements/observations (acceleromeres) 103 | if (size(Accelerometer,1) == 3), z = Accelerometer; 104 | else z = Accelerometer'; 105 | end 106 | 107 | % recompute R using the error between acceleration and the model of g 108 | % (estimate of the magnitude of a0 in a = a0 + g) 109 | a_predict = z - x_predict(1:3)*obj.g0; 110 | a_len = sqrt(a_predict'*a_predict); 111 | R = (a_len*obj.r_a2 + obj.r_acc2)*eye(3); 112 | 113 | % Kalman innovation 114 | y = z - obj.H*x_predict; 115 | S = obj.H * P_predict * obj.H' + R; 116 | 117 | % Kalman gain 118 | K = P_predict * obj.H' / S; 119 | 120 | % update a posteriori 121 | x = x_predict + K * y; 122 | 123 | % update a posteriori covariance 124 | IKH = eye(6) - K*obj.H; 125 | obj.P = IKH * P_predict * IKH' + K * R * K'; % for using any K 126 | 127 | % normalization of x & P (divide by DCM vector length) 128 | dcm_vector_length = sqrt(x(1)^2 + x(2)^2 + x(3)^2); 129 | J_33 = [x(2)^2 + x(3)^2, -x(1)*x(2), -x(1)*x(3); ... 130 | -x(1)*x(2), x(1)^2 + x(3)^2, -x(2)*x(3); ... 131 | -x(1)*x(3), -x(2)*x(3), x(1)^2 + x(2)^2]; 132 | J = [ J_33 / (dcm_vector_length^3), zeros(3,3); zeros(3,3), eye(3)]; 133 | 134 | % Laplace approximation of normalization function for x to P, J = Jacobian(f,x) 135 | % P_new = E[J*(x-x0)*(x-x0)'*J'] = J*E[(x-x0)*(x-x0)']*J' = J*P*J' 136 | obj.P = J*obj.P*J'; 137 | x(1:3) = x(1:3) ./ dcm_vector_length; 138 | obj.state = x; 139 | 140 | 141 | % compute Euler angles (not exactly a part of the extended Kalman filter) 142 | % yaw integration through full rotation matrix 143 | u_nb = u - x(4:6); 144 | if (true) 145 | % Fill rotation matrix from angular values 146 | 147 | % cy = cos(obj.yaw); %old angles (last state before integration) 148 | % sy = sin(obj.yaw); 149 | % cp = cos(obj.pitch); 150 | % sp = sin(obj.pitch); 151 | % cr = cos(obj.roll); 152 | % sr = sin(obj.roll); 153 | 154 | % % compute needed parts of rotation matrix R 155 | % R11 = cy*cp; 156 | % R12 = cy*sp*sr-sy*cr; 157 | % R13 = cy*sp*cr+sy*sr; 158 | % R21 = sy*cp; 159 | % R22 = sy*sp*sr+cy*cr; 160 | % R23 = sy*sp*cr-cy*sr; 161 | 162 | % compute needed parts of rotation matrix R using state x and yaw 163 | cy = cos(obj.yaw); %old yaw angle (last state before integration) 164 | sy = sin(obj.yaw); 165 | d = sqrt(x_last(2)^2 + x_last(3)^2); 166 | d_inv = 1 / d; 167 | 168 | % compute needed parts of rotation matrix R (state and angle based version, equivalent with the commented version above) 169 | R11 = cy * d; 170 | R12 = -(x_last(3)*sy + x_last(1)*x_last(2)*cy) * d_inv; 171 | R13 = (x_last(2)*sy - x_last(1)*x_last(3)*cy) * d_inv; 172 | R21 = sy * d; 173 | R22 = (x_last(3)*cy - x_last(1)*x_last(2)*sy) * d_inv; 174 | R23 = -(x_last(2)*cy + x_last(1)*x_last(3)*sy) * d_inv; 175 | 176 | % update needed parts of R for yaw computation 177 | R11_new = R11 + SamplePeriod*(u_nb(3)*R12 - u_nb(2)*R13); 178 | R21_new = R21 + SamplePeriod*(u_nb(3)*R22 - u_nb(2)*R23); 179 | 180 | obj.yaw = atan2(R21_new,R11_new); 181 | else 182 | % alternative method estimating the whole rotation matrix 183 | % integrate full rotation matrix (using first row estimate in memory) 184 | x1 = obj.first_row + SamplePeriod*UX'*obj.first_row; %rotate x1 by x1 x u_nb 185 | x2 = C3X * x1; %second row x2 = (state x x1) 186 | x2 = x2 ./ sqrt(x2(1)^2 + x2(2)^2 + x2(3)^2); % normalize length of the second row 187 | x1 = C3X' * x2; %recalculate first row x1 = (x2 * state) (ensure perpendicularity) 188 | obj.first_row = x1 ./ sqrt(x1(1)^2 + x1(2)^2 + x1(3)^2); % normalize length 189 | obj.yaw = atan2(x2(1),obj.first_row(1)); 190 | end 191 | 192 | %compute new pitch and roll angles from a posteriori states 193 | obj.pitch = asin(-x(1)); 194 | obj.roll = atan2(x(2),x(3)); 195 | 196 | % save the estimated non-gravitational acceleration 197 | obj.a = z - x(1:3)*obj.g0; % acceleration estimate (g reduced) 198 | end 199 | end 200 | end 201 | -------------------------------------------------------------------------------- /IMUcalibIteration.m: -------------------------------------------------------------------------------- 1 | %============================================================================ 2 | % Copyright (C) 2014, Heikki Hyyti 3 | % 4 | % Permission is hereby granted, free of charge, to any person obtaining a 5 | % copy of this software and associated documentation files (the "Software"), 6 | % to deal in the Software without restriction, including without limitation 7 | % the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | % and/or sell copies of the Software, and to permit persons to whom the 9 | % Software is furnished to do so, subject to the following conditions: 10 | % 11 | % The above copyright notice and this permission notice shall be included in 12 | % all copies or substantial portions of the Software. 13 | % 14 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | % THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | % FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | % DEALINGS IN THE SOFTWARE. 21 | %============================================================================ 22 | 23 | %source: http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=05291740 24 | % A Triaxial Accelerometer Calibration Method Using a Mathematical Model 25 | % by S.P. Won, F. Golnaraghi 26 | 27 | function [B, G] = IMUcalibIteration(S_x, S_y, S_z, B, G) 28 | % gravitation at Helsinki 29 | g0 = 9.8189; 30 | 31 | % current estimate of accelerations 32 | A_x = (S_x - B(1)) / G(1); 33 | A_y = (S_y - B(2)) / G(2); 34 | A_z = (S_z - B(3)) / G(3); 35 | 36 | % squares 37 | A_x2 = (A_x.*A_x); 38 | A_y2 = (A_y.*A_y); 39 | A_z2 = (A_z.*A_z); 40 | 41 | % error estimate 42 | E = A_x2 + A_y2 + A_z2 - (g0*g0); 43 | 44 | ACCEL = [A_x2 A_y2 A_z2 A_x A_y A_z]; 45 | 46 | CAL = ACCEL \ E; 47 | 48 | % gain change 49 | G_change2 = 1 ./ (1 - CAL(1:3)); 50 | G_change = sqrt(abs(G_change2)); 51 | 52 | % bias change 53 | B_change = CAL(4:6) .* G .* G_change2 .* 0.5; 54 | 55 | % update estimates 56 | G = G .* G_change; 57 | B = B + B_change; 58 | end -------------------------------------------------------------------------------- /MIT-LICENSE.txt: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2015 Heikki Hyyti, Aalto University 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # dcm-imu 2 | The DCM-IMU algorithm is designed for fusing low-cost triaxial MEMS gyroscope and accelerometer measurements. An extended Kalman filter is used to estimate attitude in direction cosine matrix (DCM) formation and gyroscope biases online. A variable measurement covariance method is implemented for acceleration measurements to ensure robustness against transient non-gravitational accelerations which usually induce errors to attitude estimate in ordinary IMU-algorithms. 3 | 4 | If you use the algorithm in any scientific context, please cite: Heikki Hyyti and Arto Visala, "A DCM Based Attitude Estimation Algorithm for Low-Cost MEMS IMUs," International Journal of Navigation and Observation, vol. 2015, Article ID 503814, 18 pages, 2015. http://dx.doi.org/10.1155/2015/503814 5 | 6 | If you would like to use comparison algorithms by Sebastian Madgwick, download them from http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ and copy c-implementations under c/MahonyAHRS/ (MahonyAHRS.cpp and MahonyAHRS.h) and c/MadgwickAHRS/ (MadgwickAHRS.cpp and MadgwickAHRS.h) folders. The c files have to be renamed as cpp files in order to allow Matlab to compile them correctly. Also, the DCM_IMU c code includes Eigen3 matrix library and Matlab headers. Please, install Eigen3 and change the paths inside compile.m to the paths on your computer before compiling. In addition, copy folders @MadgwickAHRS, @MahonyAHRS and quaternion_library into the main folder from the provided Matlab code by Madgwick. These files are not added into this repository as they are provided under GPL licence and this work is under MIT licence. 7 | 8 | ## Updates 9 | 2017-06-01: An alternative yaw-angle estimation method is now added to DCM_IMU matlab and c++ code versions. It integrates the whole rotation matrix without the need for computing sines or cosines from the estimated angles. However, it is currently inactivated as it is slightly slower to compute. In addition, also a more microcontroller suitable version of the DCM_IMU c code is now provided under c/DCM_IMU_uC folder. It uses only 32bit floats and works without the external matrix library (Eigen3). It is also a lot faster than the previous version. If you want to avoid installing Eigen3, you can use only the uC version of C code with Matlab. Then you should comment the normal C code version out from the compile.m file to be able to compile other than Eigen dependent codes, and replace DCM_IMU_C with DCM_IMU_uC in plotIMUsWithKuka.m and plotIMUsWithKuka_biasTest.m Matlab files. 10 | -------------------------------------------------------------------------------- /allData.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hhyyti/dcm-imu/762992befcc87be972f9d07c01d039889b545f23/allData.mat -------------------------------------------------------------------------------- /c/DCM_IMU/DCM_IMU_C.cpp: -------------------------------------------------------------------------------- 1 | #include "DCM_IMU_C.h" 2 | 3 | DCM_IMU_C::DCM_IMU_C(const double Gravity, const double *State, const double *Covariance, 4 | const double DCMVariance, const double BiasVariance, 5 | const double InitialDCMVariance, const double InitialBiasVariance, 6 | const double MeasurementVariance, const double MeasurementVarianceVariableGain) : 7 | g0(Gravity), q_dcm2(DCMVariance), q_gyro_bias2(BiasVariance), 8 | r_acc2(MeasurementVariance), r_a2(MeasurementVarianceVariableGain) { 9 | 10 | if (State == NULL) { 11 | double temp[] = DEFAULT_state; 12 | for (int i = 0; i < 6; ++i) { 13 | x[i] = temp[i]; 14 | } 15 | } 16 | else { 17 | for (int i = 0; i < 6; ++i) { 18 | x[i] = State[i]; 19 | } 20 | } 21 | 22 | P.setZero(); 23 | if (Covariance == NULL) { 24 | P(0,0) = InitialDCMVariance; 25 | P(1,1) = InitialDCMVariance; 26 | P(2,2) = InitialDCMVariance; 27 | P(3,3) = InitialBiasVariance; 28 | P(4,4) = InitialBiasVariance; 29 | P(5,5) = InitialBiasVariance; 30 | } 31 | else { 32 | for (int i = 0; i < 6; ++i) { 33 | for (int j = 0; j < 6; ++j) { 34 | P(i,j) = Covariance[6*i + j]; 35 | } 36 | } 37 | } 38 | 39 | H.setZero(); 40 | H(0,0) = g0; 41 | H(1,1) = g0; 42 | H(2,2) = g0; 43 | 44 | Q.setZero(); 45 | Q(0,0) = q_dcm2; 46 | Q(1,1) = q_dcm2; 47 | Q(2,2) = q_dcm2; 48 | Q(3,3) = q_gyro_bias2; 49 | Q(4,4) = q_gyro_bias2; 50 | Q(5,5) = q_gyro_bias2; 51 | 52 | first_row << 1.0, 0.0, 0.0; 53 | yaw = 0; 54 | pitch = 0; 55 | roll = 0; 56 | } 57 | 58 | 59 | void DCM_IMU_C::updateIMU(const double *Gyroscope, const double *Accelerometer, const double SamplePeriod) { 60 | 61 | // Process noise covariance with time dependent noise 62 | Eigen::Matrix Q_ = SamplePeriod*SamplePeriod * Q; 63 | 64 | // Control input (angular velocities from gyroscopes) 65 | Eigen::Matrix u(Gyroscope[0], Gyroscope[1], Gyroscope[2]); 66 | 67 | 68 | // "rotation operators" 69 | Eigen::Matrix C3X; 70 | C3X << 0, -x[2], x[1], 71 | x[2], 0, -x[0], 72 | -x[1], x[0], 0; 73 | 74 | Eigen::Matrix UX; 75 | UX << 0, -u[2]+x[5], u[1]-x[4], 76 | u[2]-x[5], 0, -u[0]+x[3], 77 | -u[1]+x[4], u[0]-x[3], 0; 78 | 79 | 80 | // Model generation 81 | Eigen::Matrix A; 82 | A.setZero(); 83 | A.block<3,3>(0,3) = -SamplePeriod*C3X; 84 | 85 | Eigen::Matrix B; 86 | B.setZero(); 87 | B.block<3,3>(0,0) = SamplePeriod*C3X; 88 | 89 | Eigen::Matrix F; 90 | F.setZero(); 91 | F.block<3,3>(0,0) = -SamplePeriod*UX; 92 | F.block<3,3>(0,3) = -SamplePeriod*C3X; 93 | F += Eigen::Matrix::Identity(); 94 | 95 | 96 | // Kalman a priori prediction 97 | Eigen::Matrix x_predict; 98 | x_predict = x + A*x + B*u; 99 | 100 | Eigen::Matrix P_predict; 101 | P_predict = F * P * F.transpose() + Q_; 102 | 103 | 104 | // measurements/observations (acceleromeres) 105 | Eigen::Matrix z(Accelerometer[0], Accelerometer[1], Accelerometer[2]); 106 | 107 | // recompute R using the error between acceleration and the model of g 108 | // (estimate of the magnitude of a0 in a = a0 + g) 109 | Eigen::Matrix a_predict; 110 | a_predict = z - x_predict.block<3,1>(0,0)*g0; 111 | double a_len = sqrt(a_predict[0]*a_predict[0] + a_predict[1]*a_predict[1] + a_predict[2]*a_predict[2]); 112 | 113 | Eigen::Matrix R; 114 | R = (a_len*r_a2 + r_acc2) * Eigen::Matrix::Identity(); 115 | 116 | 117 | // Kalman innovation 118 | Eigen::Matrix y; 119 | y = z - H*x_predict; 120 | 121 | Eigen::Matrix S; 122 | S = H * P_predict * H.transpose() + R; 123 | 124 | 125 | // Kalman gain 126 | Eigen::Matrix K; 127 | K = P_predict * H.transpose() * S.inverse(); 128 | 129 | //save previous x to memory 130 | Eigen::Matrix x_last = x; 131 | 132 | // update a posteriori 133 | x = x_predict + K * y; 134 | 135 | // update a posteriori covariance 136 | Eigen::Matrix IKH; 137 | IKH = Eigen::Matrix::Identity() - K*H; 138 | P = IKH * P_predict * IKH.transpose() + K * R * K.transpose(); 139 | 140 | // normalization of x & P (divide by DCM vector length) 141 | double d = sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2]); 142 | Eigen::Matrix J; 143 | J = Eigen::Matrix::Identity(); 144 | J.block<3,3>(0,0) << x[1]*x[1] + x[2]*x[2], -x[0]*x[1], -x[0]*x[2], 145 | -x[0]*x[1], x[0]*x[0] + x[2]*x[2], -x[1]*x[2], 146 | -x[0]*x[2], -x[1]*x[2], x[0]*x[0] + x[1]*x[1]; 147 | J.block<3,3>(0,0) /= (d*d*d); 148 | 149 | // Laplace approximation of normalization function for x to P, J = Jacobian(f,x) 150 | P = J* P *J.transpose(); 151 | x.block<3,1>(0,0) /= d; 152 | 153 | 154 | // compute Euler angles (not exactly a part of the extended Kalman filter) 155 | // yaw integration through full rotation matrix 156 | Eigen::Matrix u_nb; 157 | u_nb = u - x.block<3,1>(3,0); 158 | 159 | if (true) { 160 | //compute rotation matrix from previous angular and state values 161 | //double cy = cos(yaw); //old angles (last state before integration) 162 | //double sy = sin(yaw); 163 | //double cp = cos(pitch); 164 | //double sp = sin(pitch); 165 | //double cr = cos(roll); 166 | //double sr = sin(roll); 167 | 168 | // compute needed parts of rotation matrix R (angle based version) 169 | //double R11 = cy*cp; 170 | //double R12 = cy*sp*sr-sy*cr; 171 | //double R13 = cy*sp*cr+sy*sr; 172 | //double R21 = sy*cp; 173 | //double R22 = sy*sp*sr+cy*cr; 174 | //double R23 = sy*sp*cr-cy*sr; 175 | 176 | double cy = cos(yaw); //old angles (last state before integration) 177 | double sy = sin(yaw); 178 | double d = sqrt(x_last[1]*x_last[1] + x_last[2]*x_last[2]); 179 | double d_inv = 1.0 / d; 180 | 181 | // compute needed parts of rotation matrix R (state and angle based version, equivalent with the commented version above) 182 | double R11 = cy * d; 183 | double R12 = -(x_last[2]*sy + x_last[0]*x_last[1]*cy) * d_inv; 184 | double R13 = (x_last[1]*sy - x_last[0]*x_last[2]*cy) * d_inv; 185 | double R21 = sy * d; 186 | double R22 = (x_last[2]*cy - x_last[0]*x_last[1]*sy) * d_inv; 187 | double R23 = -(x_last[1]*cy + x_last[0]*x_last[2]*sy) * d_inv; 188 | 189 | // update needed parts of R for yaw computation 190 | double R11_new = R11 + SamplePeriod*(u_nb[2]*R12 - u_nb[1]*R13); 191 | double R21_new = R21 + SamplePeriod*(u_nb[2]*R22 - u_nb[1]*R23); 192 | yaw = atan2(R21_new,R11_new); 193 | } 194 | else { //alternative method estimating the whole rotation matrix 195 | //integrate full rotation matrix (using first row estimate in memory) 196 | Eigen::Matrix x1, x2; 197 | x1 = first_row + SamplePeriod * UX.transpose() * first_row; //rotate x1 by x1 x u_nb 198 | x2 = C3X * x1; //second row x2 = (state x x1) 199 | x2 /= sqrt(x2[0]*x2[0] + x2[1]*x2[1] + x2[2]*x2[2]); // normalize length of the second row 200 | x1 = C3X.transpose() * x2; // recalculate first row x1 = (x2 * state) (ensure perpendicularity) 201 | first_row = x1 / sqrt(x1[0]*x1[0] + x1[1]*x1[1] + x1[2]*x1[2]); // normalize length 202 | yaw = atan2(x2[0],first_row[0]); 203 | } 204 | 205 | // compute new pitch and roll angles from a posteriori states 206 | pitch = asin(-x[0]); 207 | roll = atan2(x[1],x[2]); 208 | 209 | // save the estimated non-gravitational acceleration 210 | a = z - x.block<3,1>(0,0)*g0; 211 | } 212 | 213 | void DCM_IMU_C::getState(double *State) { 214 | for (int i = 0; i < 6; ++i) { 215 | State[i] = x[i]; 216 | } 217 | } 218 | 219 | void DCM_IMU_C::getCovariance(double *Covariance) { 220 | for (int i = 0; i < 6; ++i) { 221 | for (int j = 0; j < 6; ++j) { 222 | Covariance[6*i + j] = P(i,j); 223 | } 224 | } 225 | } 226 | 227 | void DCM_IMU_C::getNGAcc(double *ngacc) { 228 | for (int i = 0; i < 3; ++i) { 229 | ngacc[i] = a[i]; 230 | } 231 | } 232 | 233 | -------------------------------------------------------------------------------- /c/DCM_IMU/DCM_IMU_C.h: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Copyright (C) 2017, Heikki Hyyti 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a 5 | // copy of this software and associated documentation files (the "Software"), 6 | // to deal in the Software without restriction, including without limitation 7 | // the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | // and/or sell copies of the Software, and to permit persons to whom the 9 | // Software is furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in 12 | // all copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | // DEALINGS IN THE SOFTWARE. 21 | //============================================================================ 22 | 23 | #ifndef DCM_IMU_C_H 24 | #define DCM_IMU_C_H 25 | 26 | /** 27 | * Includes 28 | */ 29 | #include "math.h" 30 | #include 31 | #include 32 | 33 | /** 34 | * Default constant values if they are not set in the constructor 35 | */ 36 | #define DEFAULT_g0 9.8189 37 | #define DEFAULT_state {0,0,1,0,0,0} 38 | #define DEFAULT_q_dcm2 (0.1*0.1) 39 | #define DEFAULT_q_gyro_bias2 (0.0001*0.0001) 40 | #define DEFAULT_r_acc2 (0.5*0.5) 41 | #define DEFAULT_r_a2 (10*10) 42 | #define DEFAULT_q_dcm2_init (1*1) 43 | #define DEFAULT_q_gyro_bias2_init (0.1*0.1) 44 | 45 | //! DCM_IMU_C class. 46 | /*! 47 | * The DCM-IMU algorithm is designed for fusing low-cost triaxial MEMS gyroscope and accelerometer measurements. An extended Kalman filter is used to estimate attitude in direction cosine matrix (DCM) formation and gyroscope biases online. A variable measurement covariance method is implemented for acceleration measurements to ensure robustness against transient non-gravitational accelerations which usually induce errors to attitude estimate in ordinary IMU-algorithms. 48 | * If you use the algorithm in any scientific context, please cite: Heikki Hyyti and Arto Visala, "A DCM Based Attitude Estimation Algorithm for Low-Cost MEMS IMUs," International Journal of Navigation and Observation, vol. 2015, Article ID 503814, 18 pages, 2015. http://dx.doi.org/10.1155/2015/503814 49 | */ 50 | class DCM_IMU_C { 51 | 52 | public: 53 | //! DCM_IMU_C constructor. 54 | /*! 55 | * Initializes DCM_IMU_C either with default values or given parameters. All parameters are in SI-units. 56 | * 57 | * @param Gravity A magnitude of gravity 58 | * @param State An initial state as a array of six doubles, DCM states and bias states. 59 | * @param Covariance A covariance matrix (size of 6x6 doubles, array of 36 doubles in row-major order). If a custom covariance matrix is given, parameters InitialDCMVariance and InitialBiasVariance are not used. 60 | * @param DCMVariance a variance for DCM state update, Q(0,0), Q(1,1), and Q(2,2) 61 | * @param BiasVariance a variance for bias state update, Q(3,3), Q(4,4), and Q(5,5) 62 | * @param InitialDCMVariance an initial variance for DCM state, P(0,0), P(1,1), and P(2,2). If Covariance matrix is given, this parameter is not used. 63 | * @param InitialBiasVariance an initial variance for bias state, P(3,3), P(4,4), and P(5,5). If Covariance matrix is given, this parameter is not used. 64 | * @param MeasurementVariance a constant part of the variance for measurement update, R(0,0), R(1,1), and R(2,2) 65 | * @param MeasurementVarianceVariableGain a gain for the variable part of the variance for measurement update, R(0,0), R(1,1), and R(2,2) 66 | */ 67 | DCM_IMU_C(const double Gravity = DEFAULT_g0, const double *State = NULL, const double *Covariance = NULL, 68 | const double DCMVariance = DEFAULT_q_dcm2, const double BiasVariance = DEFAULT_q_gyro_bias2, 69 | const double InitialDCMVariance = DEFAULT_q_dcm2_init, const double InitialBiasVariance = DEFAULT_q_gyro_bias2_init, 70 | const double MeasurementVariance = DEFAULT_r_acc2, const double MeasurementVarianceVariableGain = DEFAULT_r_a2); 71 | 72 | //! A method to perform update and give new measurements. 73 | /*! 74 | * This method is used regularly to update new gyroscope and accelerometer measurements into the system. To get best performance of the filter, please calibrate accelerometer and gyroscope readings before sending them into this method. The calibration process is documented in http://dx.doi.org/10.1155/2015/503814 75 | * In addition, please measure the used sample period as accurately as possible for each iteration (delay between current and the last data which was used at the previous update) 76 | * All parameters are in SI-units. 77 | 78 | * @param Gyroscope an array of gyroscope measurements (the length is 3 doubles, angular velocities around x, y and z axis). 79 | * @param Accelerometer an array of accelerometer measurements (the length is 3 doubles, accelerations in x, y and z axis). 80 | * @param SamplePeriod A delay between this measurement and the previous measurement in seconds. 81 | */ 82 | void updateIMU(const double *Gyroscope, const double *Accelerometer, const double SamplePeriod); 83 | 84 | //! A method to query State. 85 | /*! 86 | * @param State a 6 units long double array where the current state is stored. 87 | */ 88 | void getState(double *State); 89 | 90 | //! A method to query Covariance. 91 | /*! 92 | * @param Covariance a 36 units long double array where the current covariance is stored in row-major order. 93 | */ 94 | void getCovariance(double *Covariance); 95 | 96 | //! A method to query non-gravitational acceleration. 97 | /*! 98 | * @param a a 3 units long double array where the current non-gravitational acceleration is stored (x, y, and z axis). 99 | */ 100 | void getNGAcc(double *a); 101 | 102 | //! A method to return the yaw angle. 103 | /*! 104 | * @return The yaw angle. 105 | */ 106 | inline double getYaw() { return yaw; } 107 | 108 | //! A method to return the pitch angle. 109 | /*! 110 | * @return The pitch angle. 111 | */ 112 | inline double getPitch() { return pitch; } 113 | 114 | //! A method to return the roll angle. 115 | /*! 116 | * @return The roll angle. 117 | */ 118 | inline double getRoll() { return roll; } 119 | 120 | private: 121 | double g0; 122 | Eigen::Matrix x; 123 | double q_dcm2; 124 | double q_gyro_bias2; 125 | double r_acc2; 126 | double r_a2; 127 | Eigen::Matrix a; 128 | double yaw; 129 | double pitch; 130 | double roll; 131 | Eigen::Matrix P; 132 | Eigen::Matrix H; 133 | Eigen::Matrix Q; 134 | Eigen::Matrix first_row; 135 | }; 136 | 137 | 138 | #endif /* DCM_IMU_C_H */ 139 | -------------------------------------------------------------------------------- /c/DCM_IMU/matlabmex.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Copyright (C) 2014, Heikki Hyyti 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a 5 | // copy of this software and associated documentation files (the "Software"), 6 | // to deal in the Software without restriction, including without limitation 7 | // the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | // and/or sell copies of the Software, and to permit persons to whom the 9 | // Software is furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in 12 | // all copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | // DEALINGS IN THE SOFTWARE. 21 | //============================================================================ 22 | 23 | #include "mex.h" 24 | #include "matrix.h" 25 | #include 26 | #include "DCM_IMU_C.h" 27 | 28 | using namespace std; 29 | 30 | extern void _main(); 31 | 32 | const int numInputArgs = 3; 33 | const int numOutputArgs = 4; 34 | const int inputCols[3] = {3, 3, 1}; 35 | 36 | // mexFunction for Matlab 37 | // ----------------------------------------------------------------- 38 | void mexFunction (int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { 39 | 40 | // Check to see if we have the correct number of input and output 41 | // arguments. 42 | if (nrhs != numInputArgs) { 43 | mexErrMsgTxt("Incorrect number of input arguments, should be 3"); 44 | return; 45 | } 46 | if (nlhs != numOutputArgs) { 47 | mexErrMsgTxt("Incorrect number of output arguments, should be 4"); 48 | return; 49 | } 50 | 51 | //use row count of smallest input object 52 | int rows = mxGetM(prhs[0]); 53 | for (int i = 0; i < nrhs; ++i) { 54 | if (rows > mxGetM(prhs[i])) rows = mxGetM(prhs[i]); 55 | 56 | if (inputCols[i] != mxGetN(prhs[i])) { 57 | mexErrMsgTxt("Incorrect number of input columns, should be 3, 3 and 1"); 58 | return; 59 | } 60 | } 61 | 62 | // Create DCM_IMU with default parameters 63 | DCM_IMU_C imu; 64 | 65 | // Create the output structures. 66 | plhs[0] = mxCreateDoubleMatrix(rows,6,mxREAL); //x 67 | plhs[1] = mxCreateDoubleMatrix(rows,3,mxREAL); //ypr 68 | plhs[2] = mxCreateDoubleMatrix(rows,3,mxREAL); //a 69 | plhs[3] = mxCreateDoubleMatrix(rows,6,mxREAL); //diag(P) 70 | 71 | //inputs for one iteration 72 | double gyro[3]; 73 | double acc[3]; 74 | double dt; 75 | 76 | //input pointers 77 | double *gyro_input = mxGetPr(prhs[0]); 78 | double *acc_input = mxGetPr(prhs[1]); 79 | double *time_input = mxGetPr(prhs[2]); 80 | 81 | //outputs for one iteration 82 | double state[6]; 83 | double ypr[3]; 84 | double a[3]; 85 | double P[36]; 86 | 87 | 88 | //output pointers 89 | double *state_output = mxGetPr(plhs[0]); 90 | double *ypr_output = mxGetPr(plhs[1]); 91 | double *a_output = mxGetPr(plhs[2]); 92 | double *diagP_output = mxGetPr(plhs[3]); 93 | 94 | 95 | //(note matrix is organized differently in matlab and c++ ... ) 96 | //input[(rows*collIndex)+rowIndex] 97 | 98 | for (int i = 0; i < rows; ++i) { 99 | //read input 100 | for (int j = 0; j < 3; ++j) { 101 | gyro[j] = gyro_input[(rows*j) + i]; 102 | acc[j] = acc_input[(rows*j) + i]; 103 | } 104 | dt = time_input[i]; 105 | 106 | //update one iteration 107 | imu.updateIMU(gyro, acc, dt); 108 | 109 | //get output from filter 110 | imu.getState(state); 111 | imu.getCovariance(P); 112 | imu.getNGAcc(a); 113 | ypr[0] = imu.getYaw(); 114 | ypr[1] = imu.getPitch(); 115 | ypr[2] = imu.getRoll(); 116 | 117 | //set output 118 | for (int j = 0; j < 6; ++j) { 119 | state_output[(rows*j) + i] = state[j]; 120 | diagP_output[(rows*j) + i] = P[7*j]; 121 | } 122 | for (int j = 0; j < 3; ++j) { 123 | ypr_output[(rows*j) + i] = ypr[j]; 124 | a_output[(rows*j) + i] = a[j]; 125 | } 126 | 127 | } 128 | } 129 | 130 | -------------------------------------------------------------------------------- /c/DCM_IMU_uc/DCM_IMU_uC.cpp: -------------------------------------------------------------------------------- 1 | #include "DCM_IMU_uC.h" 2 | 3 | DCM_IMU_uC::DCM_IMU_uC(const float Gravity, const float *State, const float *Covariance, 4 | const float DCMVariance, const float BiasVariance, 5 | const float InitialDCMVariance, const float InitialBiasVariance, 6 | const float MeasurementVariance, const float MeasurementVarianceVariableGain) : 7 | g0(Gravity), q_dcm2(DCMVariance), q_gyro_bias2(BiasVariance), 8 | r_acc2(MeasurementVariance), r_a2(MeasurementVarianceVariableGain) { 9 | 10 | float temp[] = DEFAULT_state; 11 | if (State != NULL) { 12 | for (int i = 0; i < 6; ++i) { 13 | temp[i] = State[i]; 14 | } 15 | } 16 | x0 = temp[0]; 17 | x1 = temp[1]; 18 | x2 = temp[2]; 19 | x3 = temp[3]; 20 | x4 = temp[4]; 21 | x5 = temp[5]; 22 | 23 | if (Covariance == NULL) { 24 | P00 = InitialDCMVariance; 25 | P01 = 0; 26 | P02 = 0; 27 | P03 = 0; 28 | P04 = 0; 29 | P05 = 0; 30 | P11 = InitialDCMVariance; 31 | P12 = 0; 32 | P13 = 0; 33 | P14 = 0; 34 | P15 = 0; 35 | P22 = InitialDCMVariance; 36 | P23 = 0; 37 | P24 = 0; 38 | P25 = 0; 39 | P33 = InitialBiasVariance; 40 | P34 = 0; 41 | P35 = 0; 42 | P44 = InitialBiasVariance; 43 | P45 = 0; 44 | P55 = InitialBiasVariance; 45 | } 46 | else { 47 | P00 = Covariance[0]; 48 | P01 = Covariance[1]; 49 | P02 = Covariance[2]; 50 | P03 = Covariance[3]; 51 | P04 = Covariance[4]; 52 | P05 = Covariance[5]; 53 | P11 = Covariance[7]; 54 | P12 = Covariance[8]; 55 | P13 = Covariance[9]; 56 | P14 = Covariance[10]; 57 | P15 = Covariance[11]; 58 | P22 = Covariance[14]; 59 | P23 = Covariance[15]; 60 | P24 = Covariance[16]; 61 | P25 = Covariance[17]; 62 | P33 = Covariance[21]; 63 | P34 = Covariance[22]; 64 | P35 = Covariance[23]; 65 | P44 = Covariance[28]; 66 | P45 = Covariance[29]; 67 | P55 = Covariance[35]; 68 | } 69 | 70 | inv_g0 = 1.0f / g0; 71 | inv_g0_2 = inv_g0 * inv_g0; 72 | yaw = 0.0f; 73 | pitch = 0.0f; 74 | roll = 0.0f; 75 | fr0 = 1.0f; //first row for alternative rotation computation 76 | fr1 = 0.0f; //default is yaw = 0 which happens when fr = [1, 0, 0] 77 | fr2 = 0.0f; 78 | } 79 | 80 | 81 | void DCM_IMU_uC::updateIMU(const float *Gyroscope, const float *Accelerometer, const float h) { 82 | 83 | // save last state to memory for rotation estimation 84 | float x_last[3]; 85 | x_last[0] = x0; 86 | x_last[1] = x1; 87 | x_last[2] = x2; 88 | 89 | // control input (gyroscopes) 90 | float u0 = Gyroscope[0]; 91 | float u1 = Gyroscope[1]; 92 | float u2 = Gyroscope[2]; 93 | 94 | // state prediction 95 | float x_0 = x0-h*(u1*x2-u2*x1+x1*x5-x2*x4); 96 | float x_1 = x1+h*(u0*x2-u2*x0+x0*x5-x2*x3); 97 | float x_2 = x2-h*(u0*x1-u1*x0+x0*x4-x1*x3); 98 | float x_3 = x3; 99 | float x_4 = x4; 100 | float x_5 = x5; 101 | 102 | // covariance prediction 103 | float hh = h*h; 104 | float P_00 = P00-h*(P05*x1*2.0f-P04*x2*2.0f+P02*(u1-x4)*2.0f-P01*(u2-x5)*2.0f)-hh*(-q_dcm2+x2*(P45*x1-P44*x2+P24*(u1-x4)-P14*(u2-x5))+x1*(P45*x2-P55*x1-P25*(u1-x4)+P15*(u2-x5))+(u2-x5)*(P15*x1-P14*x2+P12*(u1-x4)-P11*(u2-x5))-(u1-x4)*(P25*x1-P24*x2+P22*(u1-x4)-P12*(u2-x5))); 105 | float P_01 = P01+h*(P05*x0-P03*x2-P15*x1+P14*x2+P02*(u0-x3)-P12*(u1-x4)-P00*(u2-x5)+P11*(u2-x5))+hh*(x2*(P35*x1-P34*x2+P23*(u1-x4)-P13*(u2-x5))+x0*(P45*x2-P55*x1-P25*(u1-x4)+P15*(u2-x5))+(u2-x5)*(P05*x1-P04*x2+P02*(u1-x4)-P01*(u2-x5))-(u0-x3)*(P25*x1-P24*x2+P22*(u1-x4)-P12*(u2-x5))); 106 | float P_02 = P02-h*(P04*x0-P03*x1+P25*x1-P24*x2+P01*(u0-x3)-P00*(u1-x4)+P22*(u1-x4)-P12*(u2-x5))-hh*(x1*(P35*x1-P34*x2+P23*(u1-x4)-P13*(u2-x5))-x0*(P45*x1-P44*x2+P24*(u1-x4)-P14*(u2-x5))+(u1-x4)*(P05*x1-P04*x2+P02*(u1-x4)-P01*(u2-x5))-(u0-x3)*(P15*x1-P14*x2+P12*(u1-x4)-P11*(u2-x5))); 107 | float P_03 = P03-h*(P35*x1-P34*x2+P23*(u1-x4)-P13*(u2-x5)); 108 | float P_04 = P04-h*(P45*x1-P44*x2+P24*(u1-x4)-P14*(u2-x5)); 109 | float P_05 = P05+h*(P45*x2-P55*x1-P25*(u1-x4)+P15*(u2-x5)); 110 | float P_11 = P11+h*(P15*x0*2.0f-P13*x2*2.0f+P12*(u0-x3)*2.0f-P01*(u2-x5)*2.0f)-hh*(-q_dcm2+x2*(P35*x0-P33*x2+P23*(u0-x3)-P03*(u2-x5))+x0*(P35*x2-P55*x0-P25*(u0-x3)+P05*(u2-x5))+(u2-x5)*(P05*x0-P03*x2+P02*(u0-x3)-P00*(u2-x5))-(u0-x3)*(P25*x0-P23*x2+P22*(u0-x3)-P02*(u2-x5))); 111 | float P_12 = P12-h*(P14*x0-P13*x1-P25*x0+P23*x2+P11*(u0-x3)-P01*(u1-x4)-P22*(u0-x3)+P02*(u2-x5))+hh*(x1*(P35*x0-P33*x2+P23*(u0-x3)-P03*(u2-x5))-x0*(P45*x0-P34*x2+P24*(u0-x3)-P04*(u2-x5))+(u1-x4)*(P05*x0-P03*x2+P02*(u0-x3)-P00*(u2-x5))-(u0-x3)*(P15*x0-P13*x2+P12*(u0-x3)-P01*(u2-x5))); 112 | float P_13 = P13+h*(P35*x0-P33*x2+P23*(u0-x3)-P03*(u2-x5)); 113 | float P_14 = P14+h*(P45*x0-P34*x2+P24*(u0-x3)-P04*(u2-x5)); 114 | float P_15 = P15-h*(P35*x2-P55*x0-P25*(u0-x3)+P05*(u2-x5)); 115 | float P_22 = P22-h*(P24*x0*2.0f-P23*x1*2.0f+P12*(u0-x3)*2.0f-P02*(u1-x4)*2.0f)-hh*(-q_dcm2+x1*(P34*x0-P33*x1+P13*(u0-x3)-P03*(u1-x4))+x0*(P34*x1-P44*x0-P14*(u0-x3)+P04*(u1-x4))+(u1-x4)*(P04*x0-P03*x1+P01*(u0-x3)-P00*(u1-x4))-(u0-x3)*(P14*x0-P13*x1+P11*(u0-x3)-P01*(u1-x4))); 116 | float P_23 = P23-h*(P34*x0-P33*x1+P13*(u0-x3)-P03*(u1-x4)); 117 | float P_24 = P24+h*(P34*x1-P44*x0-P14*(u0-x3)+P04*(u1-x4)); 118 | float P_25 = P25+h*(P35*x1-P45*x0-P15*(u0-x3)+P05*(u1-x4)); 119 | float P_33 = P33+hh*q_gyro_bias2; 120 | float P_34 = P34; 121 | float P_35 = P35; 122 | float P_44 = P44+hh*q_gyro_bias2; 123 | float P_45 = P45; 124 | float P_55 = P55+hh*q_gyro_bias2; 125 | 126 | // measurements (accelerometers) 127 | float z0 = Accelerometer[0] * inv_g0; 128 | float z1 = Accelerometer[1] * inv_g0; 129 | float z2 = Accelerometer[2] * inv_g0; 130 | 131 | // Kalman innovation 132 | float y0 = z0-x_0; 133 | float y1 = z1-x_1; 134 | float y2 = z2-x_2; 135 | 136 | float a_len = sqrtf(y0*y0+y1*y1+y2*y2) * g0; 137 | float r_adab = (r_acc2 + a_len*r_a2) * inv_g0_2; 138 | 139 | // innovation covariance 140 | float S00 = P_00 + r_adab; 141 | float S01 = P_01; 142 | float S02 = P_02; 143 | float S11 = P_11 + r_adab; 144 | float S12 = P_12; 145 | float S22 = P_22 + r_adab; 146 | 147 | // verify that the innovation covariance is large enough 148 | if (S00 < VARIANCE_MIN_LIMIT) S00 = VARIANCE_MIN_LIMIT; 149 | if (S11 < VARIANCE_MIN_LIMIT) S11 = VARIANCE_MIN_LIMIT; 150 | if (S22 < VARIANCE_MIN_LIMIT) S22 = VARIANCE_MIN_LIMIT; 151 | 152 | // determinant of S 153 | float det_S = -S00*(S12*S12) - (S02*S02)*S11 - (S01*S01)*S22 + S01*S02*S12*2.0f + S00*S11*S22; 154 | 155 | // Kalman gain 156 | float invPart = 1.0f / det_S; 157 | float K00 = -(S02*(P_02*S11-P_01*S12)-S01*(P_02*S12-P_01*S22)+P_00*(S12*S12)-P_00*S11*S22)*invPart; 158 | float K01 = -(S12*(P_02*S00-P_00*S02)-S01*(P_02*S02-P_00*S22)+P_01*(S02*S02)-P_01*S00*S22)*invPart; 159 | float K02 = -(S12*(P_01*S00-P_00*S01)-S02*(P_01*S01-P_00*S11)+P_02*(S01*S01)-P_02*S00*S11)*invPart; 160 | float K10 = -(S02*(P_12*S11-P_11*S12)-S01*(P_12*S12-P_11*S22)+P_01*(S12*S12)-P_01*S11*S22)*invPart; 161 | float K11 = -(S12*(P_12*S00-P_01*S02)-S01*(P_12*S02-P_01*S22)+P_11*(S02*S02)-P_11*S00*S22)*invPart; 162 | float K12 = (S12*(P_01*S01-P_11*S00)+S02*(P_11*S01-P_01*S11)-P_12*(S01*S01)+P_12*S00*S11)*invPart; 163 | float K20 = (S02*(P_12*S12-P_22*S11)+S01*(P_22*S12-P_12*S22)-P_02*(S12*S12)+P_02*S11*S22)*invPart; 164 | float K21 = (S12*(P_02*S02-P_22*S00)+S01*(P_22*S02-P_02*S22)-P_12*(S02*S02)+P_12*S00*S22)*invPart; 165 | float K22 = (S12*(P_02*S01-P_12*S00)+S02*(P_12*S01-P_02*S11)-P_22*(S01*S01)+P_22*S00*S11)*invPart; 166 | float K30 = (S02*(P_13*S12-P_23*S11)+S01*(P_23*S12-P_13*S22)-P_03*(S12*S12)+P_03*S11*S22)*invPart; 167 | float K31 = (S12*(P_03*S02-P_23*S00)+S01*(P_23*S02-P_03*S22)-P_13*(S02*S02)+P_13*S00*S22)*invPart; 168 | float K32 = (S12*(P_03*S01-P_13*S00)+S02*(P_13*S01-P_03*S11)-P_23*(S01*S01)+P_23*S00*S11)*invPart; 169 | float K40 = (S02*(P_14*S12-P_24*S11)+S01*(P_24*S12-P_14*S22)-P_04*(S12*S12)+P_04*S11*S22)*invPart; 170 | float K41 = (S12*(P_04*S02-P_24*S00)+S01*(P_24*S02-P_04*S22)-P_14*(S02*S02)+P_14*S00*S22)*invPart; 171 | float K42 = (S12*(P_04*S01-P_14*S00)+S02*(P_14*S01-P_04*S11)-P_24*(S01*S01)+P_24*S00*S11)*invPart; 172 | float K50 = (S02*(P_15*S12-P_25*S11)+S01*(P_25*S12-P_15*S22)-P_05*(S12*S12)+P_05*S11*S22)*invPart; 173 | float K51 = (S12*(P_05*S02-P_25*S00)+S01*(P_25*S02-P_05*S22)-P_15*(S02*S02)+P_15*S00*S22)*invPart; 174 | float K52 = (S12*(P_05*S01-P_15*S00)+S02*(P_15*S01-P_05*S11)-P_25*(S01*S01)+P_25*S00*S11)*invPart; 175 | 176 | // update a posteriori 177 | x0 = x_0 + K00*y0 + K01*y1 + K02*y2; 178 | x1 = x_1 + K10*y0 + K11*y1 + K12*y2; 179 | x2 = x_2 + K20*y0 + K21*y1 + K22*y2; 180 | x3 = x_3 + K30*y0 + K31*y1 + K32*y2; 181 | x4 = x_4 + K40*y0 + K41*y1 + K42*y2; 182 | x5 = x_5 + K50*y0 + K51*y1 + K52*y2; 183 | 184 | // update a posteriori covariance 185 | float K00_1 = K00 - 1.0f; 186 | float K11_1 = K11 - 1.0f; 187 | float K22_1 = K22 - 1.0f; 188 | 189 | float common1 = P_01*K00_1 + K01*P_11 + K02*P_12; 190 | float common2 = P_02*K00_1 + K01*P_12 + K02*P_22; 191 | float common3 = P_00*K00_1 + K01*P_01 + K02*P_02; 192 | float common4 = P_01*K11_1 + K10*P_00 + K12*P_02; 193 | float common5 = P_12*K11_1 + K10*P_02 + K12*P_22; 194 | float common6 = P_11*K11_1 + K10*P_01 + K12*P_12; 195 | float common7 = P_02*K22_1 + K20*P_00 + K21*P_01; 196 | float common8 = P_12*K22_1 + K20*P_01 + K21*P_11; 197 | float common9 = P_22*K22_1 + K20*P_02 + K21*P_12; 198 | float commonA = -P_03 + K30*P_00 + K31*P_01 + K32*P_02; 199 | float commonB = -P_13 + K30*P_01 + K31*P_11 + K32*P_12; 200 | float commonC = -P_23 + K30*P_02 + K31*P_12 + K32*P_22; 201 | 202 | float P__00 = K01*common1+K02*common2+(K00*K00)*r_adab+(K01*K01)*r_adab+(K02*K02)*r_adab+K00_1*common3; 203 | float P__01 = K10*common3+K12*common2+K11_1*common1+K00*K10*r_adab+K01*K11*r_adab+K02*K12*r_adab; 204 | float P__02 = K20*common3+K21*common1+K22_1*common2+K00*K20*r_adab+K01*K21*r_adab+K02*K22*r_adab; 205 | float P__03 = -P_03*K00_1+K30*common3+K31*common1+K32*common2-K01*P_13-K02*P_23+K00*K30*r_adab+K01*K31*r_adab+K02*K32*r_adab; 206 | float P__04 = -P_04*K00_1+K40*common3+K41*common1+K42*common2-K01*P_14-K02*P_24+K00*K40*r_adab+K01*K41*r_adab+K02*K42*r_adab; 207 | float P__05 = -P_05*K00_1+K50*common3+K51*common1+K52*common2-K01*P_15-K02*P_25+K00*K50*r_adab+K01*K51*r_adab+K02*K52*r_adab; 208 | float P__11 = K10*common4+K12*common5+(K10*K10)*r_adab+(K11*K11)*r_adab+(K12*K12)*r_adab+K11_1*common6; 209 | float P__12 = K20*common4+K21*common6+K22_1*common5+K10*K20*r_adab+K11*K21*r_adab+K12*K22*r_adab; 210 | float P__13 = -P_13*K11_1+K30*common4+K31*common6+K32*common5-K10*P_03-K12*P_23+K10*K30*r_adab+K11*K31*r_adab+K12*K32*r_adab; 211 | float P__14 = -P_14*K11_1+K40*common4+K41*common6+K42*common5-K10*P_04-K12*P_24+K10*K40*r_adab+K11*K41*r_adab+K12*K42*r_adab; 212 | float P__15 = -P_15*K11_1+K50*common4+K51*common6+K52*common5-K10*P_05-K12*P_25+K10*K50*r_adab+K11*K51*r_adab+K12*K52*r_adab; 213 | float P__22 = K20*common7+K21*common8+(K20*K20)*r_adab+(K21*K21)*r_adab+(K22*K22)*r_adab+K22_1*common9; 214 | float P__23 = -P_23*K22_1+K30*common7+K31*common8+K32*common9-K20*P_03-K21*P_13+K20*K30*r_adab+K21*K31*r_adab+K22*K32*r_adab; 215 | float P__24 = -P_24*K22_1+K40*common7+K41*common8+K42*common9-K20*P_04-K21*P_14+K20*K40*r_adab+K21*K41*r_adab+K22*K42*r_adab; 216 | float P__25 = -P_25*K22_1+K50*common7+K51*common8+K52*common9-K20*P_05-K21*P_15+K20*K50*r_adab+K21*K51*r_adab+K22*K52*r_adab; 217 | float P__33 = P_33+(K30*K30)*r_adab+(K31*K31)*r_adab+(K32*K32)*r_adab+K30*commonA+K31*commonB+K32*commonC-K30*P_03-K31*P_13-K32*P_23; 218 | float P__34 = P_34+K40*commonA+K41*commonB+K42*commonC-K30*P_04-K31*P_14-K32*P_24+K30*K40*r_adab+K31*K41*r_adab+K32*K42*r_adab; 219 | float P__35 = P_35+K50*commonA+K51*commonB+K52*commonC-K30*P_05-K31*P_15-K32*P_25+K30*K50*r_adab+K31*K51*r_adab+K32*K52*r_adab; 220 | float P__44 = P_44+(K40*K40)*r_adab+(K41*K41)*r_adab+(K42*K42)*r_adab+K40*(-P_04+K40*P_00+K41*P_01+K42*P_02)+K41*(-P_14+K40*P_01+K41*P_11+K42*P_12)+K42*(-P_24+K40*P_02+K41*P_12+K42*P_22)-K40*P_04-K41*P_14-K42*P_24; 221 | float P__45 = P_45+K50*(-P_04+K40*P_00+K41*P_01+K42*P_02)+K51*(-P_14+K40*P_01+K41*P_11+K42*P_12)+K52*(-P_24+K40*P_02+K41*P_12+K42*P_22)-K40*P_05-K41*P_15-K42*P_25+K40*K50*r_adab+K41*K51*r_adab+K42*K52*r_adab; 222 | float P__55 = P_55+(K50*K50)*r_adab+(K51*K51)*r_adab+(K52*K52)*r_adab+K50*(-P_05+K50*P_00+K51*P_01+K52*P_02)+K51*(-P_15+K50*P_01+K51*P_11+K52*P_12)+K52*(-P_25+K50*P_02+K51*P_12+K52*P_22)-K50*P_05-K51*P_15-K52*P_25; 223 | 224 | // Normalization of covariance 225 | float len = sqrtf(x0*x0 + x1*x1 + x2*x2); 226 | float invlen3 = 1.0 / (len*len*len); 227 | float invlen32 = (invlen3*invlen3); 228 | 229 | float x1x1_x2x2 = (x1*x1 + x2*x2); 230 | float x0x0_x2x2 = (x0*x0 + x2*x2); 231 | float x0x0_x1x1 = (x0*x0 + x1*x1); 232 | 233 | P00 = invlen32*(-x1x1_x2x2*(-P__00*x1x1_x2x2+P__01*x0*x1+P__02*x0*x2)+x0*x1*(-P__01*x1x1_x2x2+P__11*x0*x1+P__12*x0*x2)+x0*x2*(-P__02*x1x1_x2x2+P__12*x0*x1+P__22*x0*x2)); 234 | P01 = invlen32*(-x0x0_x2x2*(-P__01*x1x1_x2x2+P__11*x0*x1+P__12*x0*x2)+x0*x1*(-P__00*x1x1_x2x2+P__01*x0*x1+P__02*x0*x2)+x1*x2*(-P__02*x1x1_x2x2+P__12*x0*x1+P__22*x0*x2)); 235 | P02 = invlen32*(-x0x0_x1x1*(-P__02*x1x1_x2x2+P__12*x0*x1+P__22*x0*x2)+x0*x2*(-P__00*x1x1_x2x2+P__01*x0*x1+P__02*x0*x2)+x1*x2*(-P__01*x1x1_x2x2+P__11*x0*x1+P__12*x0*x2)); 236 | P03 = -invlen3*(-P__03*x1x1_x2x2+P__13*x0*x1+P__23*x0*x2); 237 | P04 = -invlen3*(-P__04*x1x1_x2x2+P__14*x0*x1+P__24*x0*x2); 238 | P05 = -invlen3*(-P__05*x1x1_x2x2+P__15*x0*x1+P__25*x0*x2); 239 | P11 = invlen32*(-x0x0_x2x2*(-P__11*x0x0_x2x2+P__01*x0*x1+P__12*x1*x2)+x0*x1*(-P__01*x0x0_x2x2+P__00*x0*x1+P__02*x1*x2)+x1*x2*(-P__12*x0x0_x2x2+P__02*x0*x1+P__22*x1*x2)); 240 | P12 = invlen32*(-x0x0_x1x1*(-P__12*x0x0_x2x2+P__02*x0*x1+P__22*x1*x2)+x0*x2*(-P__01*x0x0_x2x2+P__00*x0*x1+P__02*x1*x2)+x1*x2*(-P__11*x0x0_x2x2+P__01*x0*x1+P__12*x1*x2)); 241 | P13 = -invlen3*(-P__13*x0x0_x2x2+P__03*x0*x1+P__23*x1*x2); 242 | P14 = -invlen3*(-P__14*x0x0_x2x2+P__04*x0*x1+P__24*x1*x2); 243 | P15 = -invlen3*(-P__15*x0x0_x2x2+P__05*x0*x1+P__25*x1*x2); 244 | P22 = invlen32*(-x0x0_x1x1*(-P__22*x0x0_x1x1+P__02*x0*x2+P__12*x1*x2)+x0*x2*(-P__02*x0x0_x1x1+P__00*x0*x2+P__01*x1*x2)+x1*x2*(-P__12*x0x0_x1x1+P__01*x0*x2+P__11*x1*x2)); 245 | P23 = -invlen3*(-P__23*x0x0_x1x1+P__03*x0*x2+P__13*x1*x2); 246 | P24 = -invlen3*(-P__24*x0x0_x1x1+P__04*x0*x2+P__14*x1*x2); 247 | P25 = -invlen3*(-P__25*x0x0_x1x1+P__05*x0*x2+P__15*x1*x2); 248 | P33 = P__33; 249 | P34 = P__34; 250 | P35 = P__35; 251 | P44 = P__44; 252 | P45 = P__45; 253 | P55 = P__55; 254 | 255 | // increment covariance slightly at each iteration (nonoptimal but keeps the filter stable against rounding errors in 32bit float computation) 256 | P00 += VARIANCE_SAFETY_INCREMENT; 257 | P11 += VARIANCE_SAFETY_INCREMENT; 258 | P22 += VARIANCE_SAFETY_INCREMENT; 259 | P33 += VARIANCE_SAFETY_INCREMENT; 260 | P44 += VARIANCE_SAFETY_INCREMENT; 261 | P55 += VARIANCE_SAFETY_INCREMENT; 262 | 263 | // variance is required to be always at least the minimum value 264 | if (P00 < VARIANCE_MIN_LIMIT) P00 = VARIANCE_MIN_LIMIT; 265 | if (P11 < VARIANCE_MIN_LIMIT) P11 = VARIANCE_MIN_LIMIT; 266 | if (P22 < VARIANCE_MIN_LIMIT) P22 = VARIANCE_MIN_LIMIT; 267 | if (P33 < VARIANCE_MIN_LIMIT) P33 = VARIANCE_MIN_LIMIT; 268 | if (P44 < VARIANCE_MIN_LIMIT) P44 = VARIANCE_MIN_LIMIT; 269 | if (P55 < VARIANCE_MIN_LIMIT) P55 = VARIANCE_MIN_LIMIT; 270 | 271 | // normalized a posteriori state 272 | x0 = x0/len; 273 | x1 = x1/len; 274 | x2 = x2/len; 275 | 276 | 277 | // compute Euler angles (not exactly a part of the extended Kalman filter) 278 | // yaw integration through full rotation matrix 279 | float u_nb0 = u0 - x3; 280 | float u_nb1 = u1 - x4; 281 | float u_nb2 = u2 - x5; 282 | 283 | if (true) { 284 | float cy = cosf(yaw); //old angles (last state before integration) 285 | float sy = sinf(yaw); 286 | float d = sqrtf(x_last[1]*x_last[1] + x_last[2]*x_last[2]); 287 | float d_inv = 1.0f / d; 288 | 289 | // compute needed parts of rotation matrix R (state and angle based version, equivalent with the commented version above) 290 | float R11 = cy * d; 291 | float R12 = -(x_last[2]*sy + x_last[0]*x_last[1]*cy) * d_inv; 292 | float R13 = (x_last[1]*sy - x_last[0]*x_last[2]*cy) * d_inv; 293 | float R21 = sy * d; 294 | float R22 = (x_last[2]*cy - x_last[0]*x_last[1]*sy) * d_inv; 295 | float R23 = -(x_last[1]*cy + x_last[0]*x_last[2]*sy) * d_inv; 296 | 297 | // update needed parts of R for yaw computation 298 | float R11_new = R11 + h*(u_nb2*R12 - u_nb1*R13); 299 | float R21_new = R21 + h*(u_nb2*R22 - u_nb1*R23); 300 | yaw = atan2f(R21_new,R11_new); 301 | } 302 | else { //alternative method estimating the whole rotation matrix 303 | //integrate full rotation matrix (using first row estimate in memory) 304 | 305 | // calculate the second row (sr) from a rotated first row (rotation with bias corrected gyroscope measurement) 306 | float sr0 = -fr1*x_last[2]+fr2*x_last[1]-h*(x_last[1]*(fr1*u_nb0-fr0*u_nb1)+x_last[2]*(fr2*u_nb0-fr0*u_nb2)); 307 | float sr1 = fr0*x_last[2]-fr2*x_last[0]+h*(x_last[0]*(fr1*u_nb0-fr0*u_nb1)-x_last[2]*(fr2*u_nb1-fr1*u_nb2)); 308 | float sr2 = -fr0*x_last[1]+fr1*x_last[0]+h*(x_last[0]*(fr2*u_nb0-fr0*u_nb2)+x_last[1]*(fr2*u_nb1-fr1*u_nb2)); 309 | 310 | // normalize the second row 311 | float invlen = 1.0f / sqrtf(sr0*sr0 + sr1*sr1 + sr2*sr2); 312 | sr0 *= invlen; 313 | sr1 *= invlen; 314 | sr2 *= invlen; 315 | 316 | // recompute the first row (ensure perpendicularity) 317 | fr0 = sr1*x_last[2] - sr2*x_last[1]; 318 | fr1 = -sr0*x_last[2] + sr2*x_last[0]; 319 | fr2 = sr0*x_last[1] - sr1*x_last[0]; 320 | 321 | // normalize the first row 322 | invlen = 1.0f / sqrtf(fr0*fr0 + fr1*fr1 + fr2*fr2); 323 | fr0 *= invlen; 324 | fr1 *= invlen; 325 | fr2 *= invlen; 326 | 327 | // calculate yaw from first and second row 328 | yaw = atan2f(sr0,fr0); 329 | } 330 | 331 | // compute new pitch and roll angles from a posteriori states 332 | pitch = asinf(-x0); 333 | roll = atan2f(x1,x2); 334 | 335 | // save the estimated non-gravitational acceleration 336 | a0 = (z0-x0)*g0; 337 | a1 = (z1-x1)*g0; 338 | a2 = (z2-x2)*g0; 339 | } 340 | 341 | void DCM_IMU_uC::getState(float *State) { 342 | State[0] = x0; 343 | State[1] = x1; 344 | State[2] = x2; 345 | State[3] = x3; 346 | State[4] = x4; 347 | State[5] = x5; 348 | } 349 | 350 | void DCM_IMU_uC::getCovariance(float *Covariance) { 351 | Covariance[0] = P00; 352 | Covariance[1] = P01; 353 | Covariance[2] = P02; 354 | Covariance[3] = P03; 355 | Covariance[4] = P04; 356 | Covariance[5] = P05; 357 | Covariance[6] = P01; 358 | Covariance[7] = P11; 359 | Covariance[8] = P12; 360 | Covariance[9] = P13; 361 | Covariance[10] = P14; 362 | Covariance[11] = P15; 363 | Covariance[12] = P02; 364 | Covariance[13] = P12; 365 | Covariance[14] = P22; 366 | Covariance[15] = P23; 367 | Covariance[16] = P24; 368 | Covariance[17] = P25; 369 | Covariance[18] = P03; 370 | Covariance[19] = P13; 371 | Covariance[20] = P23; 372 | Covariance[21] = P33; 373 | Covariance[22] = P34; 374 | Covariance[23] = P35; 375 | Covariance[24] = P04; 376 | Covariance[25] = P14; 377 | Covariance[26] = P24; 378 | Covariance[27] = P34; 379 | Covariance[28] = P44; 380 | Covariance[29] = P45; 381 | Covariance[30] = P05; 382 | Covariance[31] = P15; 383 | Covariance[32] = P25; 384 | Covariance[33] = P35; 385 | Covariance[34] = P45; 386 | Covariance[35] = P55; 387 | } 388 | 389 | void DCM_IMU_uC::getNGAcc(float *ngacc) { 390 | ngacc[0] = a0; 391 | ngacc[1] = a1; 392 | ngacc[2] = a2; 393 | } 394 | -------------------------------------------------------------------------------- /c/DCM_IMU_uc/DCM_IMU_uC.h: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Copyright (C) 2020, Heikki Hyyti 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a 5 | // copy of this software and associated documentation files (the "Software"), 6 | // to deal in the Software without restriction, including without limitation 7 | // the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | // and/or sell copies of the Software, and to permit persons to whom the 9 | // Software is furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in 12 | // all copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | // DEALINGS IN THE SOFTWARE. 21 | //============================================================================ 22 | 23 | #ifndef DCM_IMU_UC_H 24 | #define DCM_IMU_UC_H 25 | 26 | /** 27 | * Includes 28 | */ 29 | #include "math.h" 30 | 31 | #ifndef NULL 32 | #ifdef __cplusplus 33 | #define NULL 0 34 | #else 35 | #define NULL ((void*)0) 36 | #endif 37 | #endif 38 | 39 | /** 40 | * Default constant values if they are not set in the constructor 41 | */ 42 | #define DEFAULT_g0 9.8189 43 | #define DEFAULT_state {0,0,1,0,0,0} 44 | #define DEFAULT_q_dcm2 (0.1*0.1) 45 | #define DEFAULT_q_gyro_bias2 (0.0001*0.0001) 46 | #define DEFAULT_r_acc2 (0.5*0.5) 47 | #define DEFAULT_r_a2 (10*10) 48 | #define DEFAULT_q_dcm2_init (1*1) 49 | #define DEFAULT_q_gyro_bias2_init (0.1*0.1) 50 | 51 | #define VARIANCE_MIN_LIMIT (0.0001*0.0001) //set this to a small positive number or 0 to disable the feature. 52 | #define VARIANCE_SAFETY_INCREMENT (0.00001*0.00001) //set this to a small positive number or 0 to disable the feature. 53 | 54 | //! DCM_IMU_uC class. 55 | /*! 56 | * The DCM-IMU algorithm is designed for fusing low-cost triaxial MEMS gyroscope and accelerometer measurements. An extended Kalman filter is used to estimate attitude in direction cosine matrix (DCM) formation and gyroscope biases online. A variable measurement covariance method is implemented for acceleration measurements to ensure robustness against transient non-gravitational accelerations which usually induce errors to attitude estimate in ordinary IMU-algorithms. 57 | * If you use the algorithm in any scientific context, please cite: Heikki Hyyti and Arto Visala, "A DCM Based Attitude Estimation Algorithm for Low-Cost MEMS IMUs," International Journal of Navigation and Observation, vol. 2015, Article ID 503814, 18 pages, 2015. http://dx.doi.org/10.1155/2015/503814 58 | * This updated version of micro controller c code has forced symmetry of covariance matrices that reduces computational complexity of the filter significantly. In addition, the code divides measurement with g0 before feeding it to filter which increases the stability because covariance matrix update is done with smaller weights (1 vs g0^6). 59 | * For further safety, the variance of states is limited to a small postive value and a small non-corelating noise is added to each state to keep the filter stable against rounding errors. Both of these safety additions may be disabled by defining the values to 0 in this file. 60 | */ 61 | class DCM_IMU_uC { 62 | 63 | public: 64 | //! DCM_IMU_uC constructor. 65 | /*! 66 | * Initializes DCM_IMU_uC either with default values or given parameters. All parameters are in SI-units. 67 | * 68 | * @param Gravity A magnitude of gravity 69 | * @param State An initial state as a array of six floats, DCM states and bias states. 70 | * @param Covariance A covariance matrix (size of 6x6 floats, array of 36 floats in row-major order). If a custom covariance matrix is given, parameters InitialDCMVariance and InitialBiasVariance are not used. 71 | * @param DCMVariance a variance for DCM state update, Q(0,0), Q(1,1), and Q(2,2) 72 | * @param BiasVariance a variance for bias state update, Q(3,3), Q(4,4), and Q(5,5) 73 | * @param InitialDCMVariance an initial variance for DCM state, P(0,0), P(1,1), and P(2,2). If Covariance matrix is given, this parameter is not used. 74 | * @param InitialBiasVariance an initial variance for bias state, P(3,3), P(4,4), and P(5,5). If Covariance matrix is given, this parameter is not used. 75 | * @param MeasurementVariance a constant part of the variance for measurement update, R(0,0), R(1,1), and R(2,2) 76 | * @param MeasurementVarianceVariableGain a gain for the variable part of the variance for measurement update, R(0,0), R(1,1), and R(2,2) 77 | */ 78 | DCM_IMU_uC(const float Gravity = DEFAULT_g0, const float *State = NULL, const float *Covariance = NULL, 79 | const float DCMVariance = DEFAULT_q_dcm2, const float BiasVariance = DEFAULT_q_gyro_bias2, 80 | const float InitialDCMVariance = DEFAULT_q_dcm2_init, const float InitialBiasVariance = DEFAULT_q_gyro_bias2_init, 81 | const float MeasurementVariance = DEFAULT_r_acc2, const float MeasurementVarianceVariableGain = DEFAULT_r_a2); 82 | 83 | //! A method to perform update and give new measurements. 84 | /*! 85 | * This method is used regularly to update new gyroscope and accelerometer measurements into the system. To get best performance of the filter, please calibrate accelerometer and gyroscope readings before sending them into this method. The calibration process is documented in http://dx.doi.org/10.1155/2015/503814 86 | * In addition, please measure the used sample period as accurately as possible for each iteration (delay between current and the last data which was used at the previous update) 87 | * All parameters are in SI-units. 88 | * 89 | * @param Gyroscope an array of gyroscope measurements (the length is 3 floats, angular velocities around x, y and z axis). 90 | * @param Accelerometer an array of accelerometer measurements (the length is 3 floats, accelerations in x, y and z axis). 91 | * @param SamplePeriod A delay between this measurement and the previous measurement in seconds. 92 | */ 93 | void updateIMU(const float *Gyroscope, const float *Accelerometer, const float SamplePeriod); 94 | 95 | //! A method to query State. 96 | /*! 97 | * @param State a 6 units long float array where the current state is stored. 98 | */ 99 | void getState(float *State); 100 | 101 | //! A method to query Covariance. 102 | /*! 103 | * @param Covariance a 36 units long float array where the current covariance is stored in row-major order. 104 | */ 105 | void getCovariance(float *Covariance); 106 | 107 | //! A method to query non-gravitational acceleration. 108 | /*! 109 | * @param a a 3 units long float array where the current non-gravitational acceleration is stored (x, y, and z axis). 110 | */ 111 | void getNGAcc(float *a); 112 | 113 | //! A method to return the yaw angle. 114 | /*! 115 | * @return The yaw angle. 116 | */ 117 | inline float getYaw() { return yaw; } 118 | 119 | //! A method to return the pitch angle. 120 | /*! 121 | * @return The pitch angle. 122 | */ 123 | inline float getPitch() { return pitch; } 124 | 125 | //! A method to return the roll angle. 126 | /*! 127 | * @return The roll angle. 128 | */ 129 | inline float getRoll() { return roll; } 130 | 131 | private: 132 | float g0, inv_g0, inv_g0_2; 133 | float x0, x1, x2, x3, x4, x5; 134 | float q_dcm2; 135 | float q_gyro_bias2; 136 | float r_acc2; 137 | float r_a2; 138 | float a0, a1, a2; 139 | float yaw, pitch, roll; 140 | float P00, P01, P02, P03, P04, P05; 141 | float P11, P12, P13, P14, P15; 142 | float P22, P23, P24, P25; 143 | float P33, P34, P35; 144 | float P44, P45; 145 | float P55; 146 | float fr0, fr1, fr2; 147 | }; 148 | 149 | #endif /* DCM_IMU_UC_H */ 150 | -------------------------------------------------------------------------------- /c/DCM_IMU_uc/matlabmex.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Copyright (C) 2014, Heikki Hyyti 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a 5 | // copy of this software and associated documentation files (the "Software"), 6 | // to deal in the Software without restriction, including without limitation 7 | // the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | // and/or sell copies of the Software, and to permit persons to whom the 9 | // Software is furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in 12 | // all copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | // DEALINGS IN THE SOFTWARE. 21 | //============================================================================ 22 | 23 | #include "mex.h" 24 | #include "matrix.h" 25 | #include 26 | #include "DCM_IMU_uC.h" 27 | 28 | using namespace std; 29 | 30 | extern void _main(); 31 | 32 | const int numInputArgs = 3; 33 | const int numOutputArgs = 4; 34 | const int inputCols[3] = {3, 3, 1}; 35 | 36 | // mexFunction for Matlab 37 | // ----------------------------------------------------------------- 38 | void mexFunction (int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { 39 | 40 | // Check to see if we have the correct number of input and output 41 | // arguments. 42 | if (nrhs != numInputArgs) { 43 | mexErrMsgTxt("Incorrect number of input arguments, should be 3"); 44 | return; 45 | } 46 | if (nlhs != numOutputArgs) { 47 | mexErrMsgTxt("Incorrect number of output arguments, should be 4"); 48 | return; 49 | } 50 | 51 | //use row count of smallest input object 52 | int rows = mxGetM(prhs[0]); 53 | for (int i = 0; i < nrhs; ++i) { 54 | if (rows > mxGetM(prhs[i])) rows = mxGetM(prhs[i]); 55 | 56 | if (inputCols[i] != mxGetN(prhs[i])) { 57 | mexErrMsgTxt("Incorrect number of input columns, should be 3, 3 and 1"); 58 | return; 59 | } 60 | } 61 | 62 | // Create DCM_IMU with default parameters 63 | DCM_IMU_uC imu; 64 | 65 | // Create the output structures. 66 | plhs[0] = mxCreateDoubleMatrix(rows,6,mxREAL); //x 67 | plhs[1] = mxCreateDoubleMatrix(rows,3,mxREAL); //ypr 68 | plhs[2] = mxCreateDoubleMatrix(rows,3,mxREAL); //a 69 | plhs[3] = mxCreateDoubleMatrix(rows,6,mxREAL); //diag(P) 70 | 71 | //inputs for one iteration 72 | float gyro[3]; 73 | float acc[3]; 74 | float dt; 75 | 76 | //input pointers 77 | double *gyro_input = mxGetPr(prhs[0]); 78 | double *acc_input = mxGetPr(prhs[1]); 79 | double *time_input = mxGetPr(prhs[2]); 80 | 81 | //outputs for one iteration 82 | float state[6]; 83 | float ypr[3]; 84 | float a[3]; 85 | float P[36]; 86 | 87 | 88 | //output pointers 89 | double *state_output = mxGetPr(plhs[0]); 90 | double *ypr_output = mxGetPr(plhs[1]); 91 | double *a_output = mxGetPr(plhs[2]); 92 | double *diagP_output = mxGetPr(plhs[3]); 93 | 94 | 95 | //(note matrix is organized differently in matlab and c++ ... ) 96 | //input[(rows*collIndex)+rowIndex] 97 | 98 | for (int i = 0; i < rows; ++i) { 99 | //read input 100 | for (int j = 0; j < 3; ++j) { 101 | gyro[j] = gyro_input[(rows*j) + i]; 102 | acc[j] = acc_input[(rows*j) + i]; 103 | } 104 | dt = time_input[i]; 105 | 106 | //update one iteration 107 | imu.updateIMU(gyro, acc, dt); 108 | 109 | //get output from filter 110 | imu.getState(state); 111 | imu.getCovariance(P); 112 | imu.getNGAcc(a); 113 | ypr[0] = imu.getYaw(); 114 | ypr[1] = imu.getPitch(); 115 | ypr[2] = imu.getRoll(); 116 | 117 | //set output 118 | for (int j = 0; j < 6; ++j) { 119 | state_output[(rows*j) + i] = state[j]; 120 | diagP_output[(rows*j) + i] = P[7*j]; 121 | } 122 | for (int j = 0; j < 3; ++j) { 123 | ypr_output[(rows*j) + i] = ypr[j]; 124 | a_output[(rows*j) + i] = a[j]; 125 | } 126 | 127 | } 128 | } 129 | 130 | -------------------------------------------------------------------------------- /c/MadgwickAHRS/matlabmex.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Copyright (C) 2014, Heikki Hyyti 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a 5 | // copy of this software and associated documentation files (the "Software"), 6 | // to deal in the Software without restriction, including without limitation 7 | // the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | // and/or sell copies of the Software, and to permit persons to whom the 9 | // Software is furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in 12 | // all copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | // DEALINGS IN THE SOFTWARE. 21 | //============================================================================ 22 | 23 | #include "mex.h" 24 | #include "matrix.h" 25 | #include 26 | #include "MadgwickAHRS.h" 27 | 28 | using namespace std; 29 | 30 | extern void _main(); 31 | 32 | const int numInputArgs = 2; 33 | const int numOutputArgs = 1; 34 | const int inputCols[2] = {3, 3}; 35 | 36 | // mexFunction for Matlab 37 | // ----------------------------------------------------------------- 38 | void mexFunction (int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { 39 | 40 | if (sizeof(float) * CHAR_BIT != 32) { //Cause error on systems with 64bit float as invSqrt will not work properly! 41 | mexErrMsgTxt("32 bit floats are required for invSqrt in the included code!"); 42 | return; 43 | } 44 | 45 | if (sizeof(long) * CHAR_BIT != 32) { //Madgwick's code uses long for 32bit signed integer. It is 64bit in LP64 architecture and the invSqrt miscalculates. 46 | mexPrintf("Warning: invSqrt does not work with %d bit longs\nPlease change it to int or int32_t in the included code!\n", sizeof(long) * CHAR_BIT); 47 | } 48 | 49 | // Check to see if we have the correct number of input and output 50 | // arguments. 51 | if (nrhs != numInputArgs) { 52 | mexErrMsgTxt("Incorrect number of input arguments, should be 2"); 53 | return; 54 | } 55 | if (nlhs != numOutputArgs) { 56 | mexErrMsgTxt("Incorrect number of output arguments, should be 1"); 57 | return; 58 | } 59 | 60 | //use row count of smallest input object 61 | int rows = mxGetM(prhs[0]); 62 | for (int i = 0; i < nrhs; ++i) { 63 | if (rows > mxGetM(prhs[i])) rows = mxGetM(prhs[i]); 64 | 65 | if (inputCols[i] != mxGetN(prhs[i])) { 66 | mexErrMsgTxt("Incorrect number of input columns, should be 3 and 3"); 67 | return; 68 | } 69 | } 70 | 71 | 72 | // Create the output structures. 73 | plhs[0] = mxCreateDoubleMatrix(rows,4,mxREAL); //q 74 | 75 | //inputs for one iteration 76 | double gyro[3]; 77 | double acc[3]; 78 | 79 | //input pointers 80 | double *gyro_input = mxGetPr(prhs[0]); 81 | double *acc_input = mxGetPr(prhs[1]); 82 | 83 | //outputs for one iteration 84 | double q[4]; 85 | 86 | //output pointers 87 | double *q_output = mxGetPr(plhs[0]); 88 | 89 | //(note matrix is organized differently in matlab and c++ ... ) 90 | //input[(rows*collIndex)+rowIndex] 91 | 92 | for (int i = 0; i < rows; ++i) { 93 | //read input 94 | for (int j = 0; j < 3; ++j) { 95 | gyro[j] = gyro_input[(rows*j) + i]; 96 | acc[j] = acc_input[(rows*j) + i]; 97 | } 98 | 99 | //update one iteration 100 | MadgwickAHRSupdateIMU(gyro[0], gyro[1], gyro[2], acc[0], acc[1], acc[2]); 101 | 102 | //get output from filter 103 | q[0] = q0; 104 | q[1] = q1; 105 | q[2] = q2; 106 | q[3] = q3; 107 | 108 | //set output 109 | for (int j = 0; j < 4; ++j) { 110 | q_output[(rows*j) + i] = q[j]; 111 | } 112 | } 113 | } 114 | 115 | -------------------------------------------------------------------------------- /c/MahonyAHRS/matlabmex.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Copyright (C) 2014, Heikki Hyyti 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a 5 | // copy of this software and associated documentation files (the "Software"), 6 | // to deal in the Software without restriction, including without limitation 7 | // the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | // and/or sell copies of the Software, and to permit persons to whom the 9 | // Software is furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in 12 | // all copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | // DEALINGS IN THE SOFTWARE. 21 | //============================================================================ 22 | 23 | #include "mex.h" 24 | #include "matrix.h" 25 | #include 26 | #include "MahonyAHRS.h" 27 | 28 | using namespace std; 29 | 30 | extern void _main(); 31 | 32 | const int numInputArgs = 2; 33 | const int numOutputArgs = 1; 34 | const int inputCols[2] = {3, 3}; 35 | 36 | // mexFunction for Matlab 37 | // ----------------------------------------------------------------- 38 | void mexFunction (int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { 39 | 40 | if (sizeof(float) * CHAR_BIT != 32) { //Cause error on systems with 64bit float as invSqrt will not work properly! 41 | mexErrMsgTxt("32 bit floats are required for invSqrt in the included code!"); 42 | return; 43 | } 44 | 45 | if (sizeof(long) * CHAR_BIT != 32) { //Madgwick's code uses long for 32bit signed integer. It is 64bit in LP64 architecture and the invSqrt miscalculates. 46 | mexPrintf("Warning: invSqrt does not work with %d bit longs\nPlease change it to int or int32_t in the included code!\n", sizeof(long) * CHAR_BIT); 47 | } 48 | 49 | // Check to see if we have the correct number of input and output 50 | // arguments. 51 | if (nrhs != numInputArgs) { 52 | mexErrMsgTxt("Incorrect number of input arguments, should be 2"); 53 | return; 54 | } 55 | if (nlhs != numOutputArgs) { 56 | mexErrMsgTxt("Incorrect number of output arguments, should be 1"); 57 | return; 58 | } 59 | 60 | //use row count of smallest input object 61 | int rows = mxGetM(prhs[0]); 62 | for (int i = 0; i < nrhs; ++i) { 63 | if (rows > mxGetM(prhs[i])) rows = mxGetM(prhs[i]); 64 | 65 | if (inputCols[i] != mxGetN(prhs[i])) { 66 | mexErrMsgTxt("Incorrect number of input columns, should be 3 and 3"); 67 | return; 68 | } 69 | } 70 | 71 | 72 | // Create the output structures. 73 | plhs[0] = mxCreateDoubleMatrix(rows,4,mxREAL); //q 74 | 75 | //inputs for one iteration 76 | double gyro[3]; 77 | double acc[3]; 78 | 79 | //input pointers 80 | double *gyro_input = mxGetPr(prhs[0]); 81 | double *acc_input = mxGetPr(prhs[1]); 82 | 83 | //outputs for one iteration 84 | double q[4]; 85 | 86 | //output pointers 87 | double *q_output = mxGetPr(plhs[0]); 88 | 89 | //(note matrix is organized differently in matlab and c++ ... ) 90 | //input[(rows*collIndex)+rowIndex] 91 | 92 | for (int i = 0; i < rows; ++i) { 93 | //read input 94 | for (int j = 0; j < 3; ++j) { 95 | gyro[j] = gyro_input[(rows*j) + i]; 96 | acc[j] = acc_input[(rows*j) + i]; 97 | } 98 | 99 | //update one iteration 100 | MahonyAHRSupdateIMU(gyro[0], gyro[1], gyro[2], acc[0], acc[1], acc[2]); 101 | 102 | //get output from filter 103 | q[0] = q0; 104 | q[1] = q1; 105 | q[2] = q2; 106 | q[3] = q3; 107 | 108 | //set output 109 | for (int j = 0; j < 4; ++j) { 110 | q_output[(rows*j) + i] = q[j]; 111 | } 112 | } 113 | } 114 | 115 | -------------------------------------------------------------------------------- /c/compile.m: -------------------------------------------------------------------------------- 1 | %============================================================================ 2 | % Copyright (C) 2017, Heikki Hyyti 3 | % 4 | % Permission is hereby granted, free of charge, to any person obtaining a 5 | % copy of this software and associated documentation files (the "Software"), 6 | % to deal in the Software without restriction, including without limitation 7 | % the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | % and/or sell copies of the Software, and to permit persons to whom the 9 | % Software is furnished to do so, subject to the following conditions: 10 | % 11 | % The above copyright notice and this permission notice shall be included in 12 | % all copies or substantial portions of the Software. 13 | % 14 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | % THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | % FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | % DEALINGS IN THE SOFTWARE. 21 | %============================================================================ 22 | 23 | 24 | disp('Compiling DCM-IMU...'); 25 | mex -lm -I/usr/include/eigen3 -I./DCM_IMU -I/usr/local/matlab/extern/include ... 26 | -output ../DCM_IMU_C DCM_IMU/matlabmex.cpp DCM_IMU/DCM_IMU_C.cpp; 27 | 28 | disp('Compiling DCM-IMU-uc...'); 29 | mex -lm -I./DCM_IMU_uc -I/usr/local/matlab/extern/include ... 30 | -output ../DCM_IMU_uC DCM_IMU_uc/matlabmex.cpp DCM_IMU_uc/DCM_IMU_uC.cpp; 31 | 32 | if (exist('MadgwickAHRS/MadgwickAHRS.cpp', 'file') && exist('MadgwickAHRS/MadgwickAHRS.h', 'file')) 33 | disp('Compiling Madgwick AHRS...'); 34 | mex -lm -I/usr/local/matlab/extern/include -I./MadgwickAHRS ... 35 | -output ../Madgwick_IMU_C MadgwickAHRS/matlabmex.cpp MadgwickAHRS/MadgwickAHRS.cpp; 36 | else 37 | disp('"MadgwickAHRS/MadgwickAHRS.cpp" or "MadgwickAHRS/MadgwickAHRS.h" not found.'); 38 | disp('To get reference algorithms, download their implementations from here:'); 39 | disp('http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/'); 40 | end 41 | 42 | if (exist('MahonyAHRS/MahonyAHRS.cpp', 'file') && exist('MahonyAHRS/MahonyAHRS.h', 'file')) 43 | disp('Compiling Mahony AHRS...'); 44 | mex -lm -I/usr/local/matlab/extern/include -I./MahonyAHRS ... 45 | -output ../Mahony_IMU_C MahonyAHRS/matlabmex.cpp MahonyAHRS/MahonyAHRS.cpp; 46 | else 47 | disp('"MahonyAHRS/MahonyAHRS.cpp" or "MahonyAHRS/MahonyAHRS.h" not found.'); 48 | disp('To get reference algorithms, download their implementations from here:'); 49 | disp('http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/'); 50 | end 51 | -------------------------------------------------------------------------------- /fitRegressionLine.m: -------------------------------------------------------------------------------- 1 | %============================================================================ 2 | % Copyright (C) 2014, Heikki Hyyti 3 | % 4 | % Permission is hereby granted, free of charge, to any person obtaining a 5 | % copy of this software and associated documentation files (the "Software"), 6 | % to deal in the Software without restriction, including without limitation 7 | % the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | % and/or sell copies of the Software, and to permit persons to whom the 9 | % Software is furnished to do so, subject to the following conditions: 10 | % 11 | % The above copyright notice and this permission notice shall be included in 12 | % all copies or substantial portions of the Software. 13 | % 14 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | % THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | % FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | % DEALINGS IN THE SOFTWARE. 21 | %============================================================================ 22 | 23 | function [a,b] = fitRegressionLine(x, y) 24 | A11 = x' * x; 25 | A12 = sum(x); 26 | A21 = A12; 27 | A22 = size(x,1); 28 | 29 | B11 = y(:,1)' * x; 30 | B21 = sum(y(:,1)); 31 | 32 | a = (A12*B21 - A22*B11) / (A12*A21 - A11*A22); 33 | b = (A11*B21 - A21*B11) / (A11*A22 - A12*A21); 34 | end 35 | -------------------------------------------------------------------------------- /imusWithKukaCalibration.m: -------------------------------------------------------------------------------- 1 | %============================================================================ 2 | % Copyright (C) 2015, Heikki Hyyti 3 | % 4 | % Permission is hereby granted, free of charge, to any person obtaining a 5 | % copy of this software and associated documentation files (the "Software"), 6 | % to deal in the Software without restriction, including without limitation 7 | % the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | % and/or sell copies of the Software, and to permit persons to whom the 9 | % Software is furnished to do so, subject to the following conditions: 10 | % 11 | % The above copyright notice and this permission notice shall be included in 12 | % all copies or substantial portions of the Software. 13 | % 14 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | % THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | % FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | % DEALINGS IN THE SOFTWARE. 21 | %============================================================================ 22 | 23 | % acceleration data calibration 1 is for IMU1 (IL), 2 for IMU2 (IL) 24 | calibTimes = [9, 23; 26, 40; 42, 56; 59, 73; 76, 89; 93, 107]' + 4; 25 | angleCalibTimes = [116, 225; 234, 345; 351, 464]'; 26 | 27 | calibTimes2 = calibTimes + 763; 28 | angleCalibTimes2 = angleCalibTimes + 763; 29 | 30 | minCalibTime = min(calibTimes(:)); 31 | maxCalibTime = max(angleCalibTimes2(:)); 32 | 33 | ILdelay = 0.03; %seconds, delay measured in InertiaLink data 34 | SFdelay = 0; %seconds, delay measured in SparkFun data 35 | 36 | % calibration point 1 37 | 38 | S1_x = zeros(6,1); 39 | S1_y = zeros(6,1); 40 | S1_z = zeros(6,1); 41 | w1_Bx = zeros(6,1); 42 | w1_By = zeros(6,1); 43 | w1_Bz = zeros(6,1); 44 | T_1 = zeros(6,1); 45 | 46 | S2_x = zeros(6,1); 47 | S2_y = zeros(6,1); 48 | S2_z = zeros(6,1); 49 | w2_Bx = zeros(6,1); 50 | w2_By = zeros(6,1); 51 | w2_Bz = zeros(6,1); 52 | 53 | for i = 1:6 54 | SFrange = SFtime > calibTimes(1,i) & SFtime < calibTimes(2,i); 55 | ILrange = ILtime > calibTimes(1,i) & ILtime < calibTimes(2,i); 56 | 57 | S1_x(i) = mean(data.SparkFun6DOF.acc_x(SFrange)); 58 | S1_y(i) = mean(data.SparkFun6DOF.acc_y(SFrange)); 59 | S1_z(i) = mean(data.SparkFun6DOF.acc_z(SFrange)); 60 | w1_Bx(i) = mean(data.SparkFun6DOF.w_x(SFrange)); 61 | w1_By(i) = mean(data.SparkFun6DOF.w_y(SFrange)); 62 | w1_Bz(i) = mean(data.SparkFun6DOF.w_z(SFrange)); 63 | T_1(i) = mean(data.SparkFun6DOF.temperature(SFrange)); 64 | 65 | S2_x(i) = mean(data.InertiaLink.acc_x(ILrange)); 66 | S2_y(i) = mean(data.InertiaLink.acc_y(ILrange)); 67 | S2_z(i) = mean(data.InertiaLink.acc_z(ILrange)); 68 | w2_Bx(i) = mean(data.InertiaLink.w_x(ILrange)); 69 | w2_By(i) = mean(data.InertiaLink.w_y(ILrange)); 70 | w2_Bz(i) = mean(data.InertiaLink.w_z(ILrange)); 71 | end 72 | T1_acc = mean(T_1); %average temperature during acceleration calibration 73 | 74 | % initialize biases and gains 75 | % (assume that bias is near zero and gain near one) 76 | B1 = zeros(3,1); 77 | B2 = zeros(3,1); 78 | 79 | G1 = ones(3,1); 80 | G2 = ones(3,1); 81 | 82 | % run iterative calibration algorithm for both Temperatures 83 | for i = 1:1000 84 | [B1_new, G1_new] = IMUcalibIteration(S1_x, S1_y, S1_z, B1, G1); 85 | [B2_new, G2_new] = IMUcalibIteration(S2_x, S2_y, S2_z, B2, G2); 86 | 87 | Differences = abs(B1_new - B1) + abs(G1_new - G1) + ... 88 | abs(B2_new - B2) + abs(G2_new - G2); 89 | if (Differences < 0.000001) 90 | break; 91 | end 92 | 93 | B1 = B1_new; 94 | G1 = G1_new; 95 | B2 = B2_new; 96 | G2 = G2_new; 97 | end 98 | 99 | 100 | % calibrate gyroscopes by comparing to KUKA data 101 | 102 | % resample Kuka angular velocities to SFtime 103 | minTime = min(angleCalibTimes(:)); 104 | maxTime = max(angleCalibTimes(:)); 105 | SFrange = SFtime > minTime & SFtime < maxTime; 106 | T1_gyro = mean(data.SparkFun6DOF.temperature(SFrange)); 107 | angleCalibTime_SF = SFtime(SFrange); 108 | 109 | w_x_in_SF = resample(timeseries(gyro(:,1), Ktime+SFdelay),angleCalibTime_SF); 110 | w_x_in_SF = w_x_in_SF.Data; 111 | w_y_in_SF = resample(timeseries(gyro(:,2), Ktime+SFdelay),angleCalibTime_SF); 112 | w_y_in_SF = w_y_in_SF.Data; 113 | w_z_in_SF = resample(timeseries(gyro(:,3), Ktime+SFdelay),angleCalibTime_SF); 114 | w_z_in_SF = w_z_in_SF.Data; 115 | 116 | w_x_error_SF = data.SparkFun6DOF.w_x(SFrange) - w_x_in_SF; 117 | w_y_error_SF = data.SparkFun6DOF.w_y(SFrange) - w_y_in_SF; 118 | w_z_error_SF = data.SparkFun6DOF.w_z(SFrange) - w_z_in_SF; 119 | 120 | %filter out range where reference angular velocity is near zero 121 | filt_dist = 0.0055; 122 | x_filt_SF = (w_x_in_SF > filt_dist) | (w_x_in_SF < -filt_dist); 123 | y_filt_SF = (w_y_in_SF > filt_dist) | (w_y_in_SF < -filt_dist); 124 | z_filt_SF = (w_z_in_SF > filt_dist) | (w_z_in_SF < -filt_dist); 125 | 126 | %resample Kuka angular velocities to ILtime 127 | ILrange = ILtime > minTime & ILtime < maxTime; 128 | angleCalibTime_IL = ILtime(ILrange); 129 | 130 | w_x_in_IL = resample(timeseries(gyro(:,1), Ktime+ILdelay),angleCalibTime_IL); 131 | w_x_in_IL = w_x_in_IL.Data; 132 | w_y_in_IL = resample(timeseries(gyro(:,2), Ktime+ILdelay),angleCalibTime_IL); 133 | w_y_in_IL = w_y_in_IL.Data; 134 | w_z_in_IL = resample(timeseries(gyro(:,3), Ktime+ILdelay),angleCalibTime_IL); 135 | w_z_in_IL = w_z_in_IL.Data; 136 | 137 | w_x_error_IL = data.InertiaLink.w_x(ILrange) - w_x_in_IL; 138 | w_y_error_IL = data.InertiaLink.w_y(ILrange) - w_y_in_IL; 139 | w_z_error_IL = data.InertiaLink.w_z(ILrange) - w_z_in_IL; 140 | 141 | %filter out range where reference angular velocity is near zero 142 | x_filt_IL = (w_x_in_IL > filt_dist) | (w_x_in_IL < -filt_dist); 143 | y_filt_IL = (w_y_in_IL > filt_dist) | (w_y_in_IL < -filt_dist); 144 | z_filt_IL = (w_z_in_IL > filt_dist) | (w_z_in_IL < -filt_dist); 145 | 146 | [w1_Gx, w1_Bx] = fitRegressionLine(w_x_in_SF(x_filt_SF), w_x_in_SF(x_filt_SF)+w_x_error_SF(x_filt_SF)); 147 | [w2_Gx, w2_Bx] = fitRegressionLine(w_x_in_IL(x_filt_IL), w_x_in_IL(x_filt_IL)+w_x_error_IL(x_filt_IL)); 148 | [w1_Gy, w1_By] = fitRegressionLine(w_y_in_SF(y_filt_SF), w_y_in_SF(y_filt_SF)+w_y_error_SF(y_filt_SF)); 149 | [w2_Gy, w2_By] = fitRegressionLine(w_y_in_IL(y_filt_IL), w_y_in_IL(y_filt_IL)+w_y_error_IL(y_filt_IL)); 150 | [w1_Gz, w1_Bz] = fitRegressionLine(w_z_in_SF(z_filt_SF), w_z_in_SF(z_filt_SF)+w_z_error_SF(z_filt_SF)); 151 | [w2_Gz, w2_Bz] = fitRegressionLine(w_z_in_IL(z_filt_IL), w_z_in_IL(z_filt_IL)+w_z_error_IL(z_filt_IL)); 152 | 153 | 154 | % calibration point 2 155 | 156 | S1b_x = zeros(6,1); 157 | S1b_y = zeros(6,1); 158 | S1b_z = zeros(6,1); 159 | w1b_Bx = zeros(6,1); 160 | w1b_By = zeros(6,1); 161 | w1b_Bz = zeros(6,1); 162 | T_2 = zeros(6,1); 163 | 164 | S2b_x = zeros(6,1); 165 | S2b_y = zeros(6,1); 166 | S2b_z = zeros(6,1); 167 | w2b_Bx = zeros(6,1); 168 | w2b_By = zeros(6,1); 169 | w2b_Bz = zeros(6,1); 170 | 171 | for i = 1:6 172 | SFrange = SFtime > calibTimes2(1,i) & SFtime < calibTimes2(2,i); 173 | ILrange = ILtime > calibTimes2(1,i) & ILtime < calibTimes2(2,i); 174 | 175 | S1b_x(i) = mean(data.SparkFun6DOF.acc_x(SFrange)); 176 | S1b_y(i) = mean(data.SparkFun6DOF.acc_y(SFrange)); 177 | S1b_z(i) = mean(data.SparkFun6DOF.acc_z(SFrange)); 178 | w1b_Bx(i) = mean(data.SparkFun6DOF.w_x(SFrange)); 179 | w1b_By(i) = mean(data.SparkFun6DOF.w_y(SFrange)); 180 | w1b_Bz(i) = mean(data.SparkFun6DOF.w_z(SFrange)); 181 | T_2(i) = mean(data.SparkFun6DOF.temperature(SFrange)); 182 | 183 | S2b_x(i) = mean(data.InertiaLink.acc_x(ILrange)); 184 | S2b_y(i) = mean(data.InertiaLink.acc_y(ILrange)); 185 | S2b_z(i) = mean(data.InertiaLink.acc_z(ILrange)); 186 | w2b_Bx(i) = mean(data.InertiaLink.w_x(ILrange)); 187 | w2b_By(i) = mean(data.InertiaLink.w_y(ILrange)); 188 | w2b_Bz(i) = mean(data.InertiaLink.w_z(ILrange)); 189 | end 190 | T2_acc = mean(T_2); %average temperature during acceleration calibration 191 | 192 | % initialize biases and gains 193 | % (assume that bias is near zero and gain near one) 194 | B1b = zeros(3,1); 195 | B2b = zeros(3,1); 196 | 197 | G1b = ones(3,1); 198 | G2b = ones(3,1); 199 | 200 | % run iterative calibration algorithm for both Temperatures 201 | for i = 1:1000 202 | [B1b_new, G1b_new] = IMUcalibIteration(S1b_x, S1b_y, S1b_z, B1b, G1b); 203 | [B2b_new, G2b_new] = IMUcalibIteration(S2b_x, S2b_y, S2b_z, B2b, G2b); 204 | 205 | Differences = abs(B1b_new - B1b) + abs(G1b_new - G1b) + ... 206 | abs(B2b_new - B2b) + abs(G2b_new - G2b); 207 | if (Differences < 0.000001) 208 | break; 209 | end 210 | 211 | B1b = B1b_new; 212 | G1b = G1b_new; 213 | B2b = B2b_new; 214 | G2b = G2b_new; 215 | end 216 | 217 | % calibrate gyroscopes by comparing to KUKA data 218 | 219 | % resample Kuka angular velocities to SFtime 220 | minTime = min(angleCalibTimes2(:)); 221 | maxTime = max(angleCalibTimes2(:)); 222 | SFrange = SFtime > minTime & SFtime < maxTime; 223 | T2_gyro = mean(data.SparkFun6DOF.temperature(SFrange)); 224 | angleCalibTime_SF = SFtime(SFrange); 225 | 226 | w_x_in_SF = resample(timeseries(gyro(:,1), Ktime+SFdelay),angleCalibTime_SF); 227 | w_x_in_SF = w_x_in_SF.Data; 228 | w_y_in_SF = resample(timeseries(gyro(:,2), Ktime+SFdelay),angleCalibTime_SF); 229 | w_y_in_SF = w_y_in_SF.Data; 230 | w_z_in_SF = resample(timeseries(gyro(:,3), Ktime+SFdelay),angleCalibTime_SF); 231 | w_z_in_SF = w_z_in_SF.Data; 232 | 233 | w_x_error_SF = data.SparkFun6DOF.w_x(SFrange) - w_x_in_SF; 234 | w_y_error_SF = data.SparkFun6DOF.w_y(SFrange) - w_y_in_SF; 235 | w_z_error_SF = data.SparkFun6DOF.w_z(SFrange) - w_z_in_SF; 236 | 237 | % filter out range where reference angular velocity is near zero 238 | filt_dist = 0.0055; 239 | x_filt_SF = (w_x_in_SF > filt_dist) | (w_x_in_SF < -filt_dist); 240 | y_filt_SF = (w_y_in_SF > filt_dist) | (w_y_in_SF < -filt_dist); 241 | z_filt_SF = (w_z_in_SF > filt_dist) | (w_z_in_SF < -filt_dist); 242 | 243 | % resample Kuka angular velocities to ILtime 244 | ILrange = ILtime > minTime & ILtime < maxTime; 245 | angleCalibTime_IL = ILtime(ILrange); 246 | 247 | w_x_in_IL = resample(timeseries(gyro(:,1), Ktime+ILdelay),angleCalibTime_IL); 248 | w_x_in_IL = w_x_in_IL.Data; 249 | w_y_in_IL = resample(timeseries(gyro(:,2), Ktime+ILdelay),angleCalibTime_IL); 250 | w_y_in_IL = w_y_in_IL.Data; 251 | w_z_in_IL = resample(timeseries(gyro(:,3), Ktime+ILdelay),angleCalibTime_IL); 252 | w_z_in_IL = w_z_in_IL.Data; 253 | 254 | w_x_error_IL = data.InertiaLink.w_x(ILrange) - w_x_in_IL; 255 | w_y_error_IL = data.InertiaLink.w_y(ILrange) - w_y_in_IL; 256 | w_z_error_IL = data.InertiaLink.w_z(ILrange) - w_z_in_IL; 257 | 258 | % filter out range where reference angular velocity is near zero 259 | x_filt_IL = (w_x_in_IL > filt_dist) | (w_x_in_IL < -filt_dist); 260 | y_filt_IL = (w_y_in_IL > filt_dist) | (w_y_in_IL < -filt_dist); 261 | z_filt_IL = (w_z_in_IL > filt_dist) | (w_z_in_IL < -filt_dist); 262 | 263 | [w1b_Gx, w1b_Bx] = fitRegressionLine(w_x_in_SF(x_filt_SF), w_x_in_SF(x_filt_SF)+w_x_error_SF(x_filt_SF)); 264 | [w2b_Gx, w2b_Bx] = fitRegressionLine(w_x_in_IL(x_filt_IL), w_x_in_IL(x_filt_IL)+w_x_error_IL(x_filt_IL)); 265 | [w1b_Gy, w1b_By] = fitRegressionLine(w_y_in_SF(y_filt_SF), w_y_in_SF(y_filt_SF)+w_y_error_SF(y_filt_SF)); 266 | [w2b_Gy, w2b_By] = fitRegressionLine(w_y_in_IL(y_filt_IL), w_y_in_IL(y_filt_IL)+w_y_error_IL(y_filt_IL)); 267 | [w1b_Gz, w1b_Bz] = fitRegressionLine(w_z_in_SF(z_filt_SF), w_z_in_SF(z_filt_SF)+w_z_error_SF(z_filt_SF)); 268 | [w2b_Gz, w2b_Bz] = fitRegressionLine(w_z_in_IL(z_filt_IL), w_z_in_IL(z_filt_IL)+w_z_error_IL(z_filt_IL)); 269 | 270 | % accelerometer parameters for sparkfun 271 | a1_b_Ax = (B1b(1) - B1(1)) / (T2_acc - T1_acc); 272 | a1_b_Ay = (B1b(2) - B1(2)) / (T2_acc - T1_acc); 273 | a1_b_Az = (B1b(3) - B1(3)) / (T2_acc - T1_acc); 274 | 275 | a1_b_Bx = B1(1) - a1_b_Ax*T1_acc; 276 | a1_b_By = B1(2) - a1_b_Ay*T1_acc; 277 | a1_b_Bz = B1(3) - a1_b_Az*T1_acc; 278 | 279 | a1_g_Ax = (G1b(1) - G1(1)) / (T2_acc - T1_acc); 280 | a1_g_Ay = (G1b(2) - G1(2)) / (T2_acc - T1_acc); 281 | a1_g_Az = (G1b(3) - G1(3)) / (T2_acc - T1_acc); 282 | 283 | a1_g_Bx = G1(1) - a1_g_Ax*T1_acc; 284 | a1_g_By = G1(2) - a1_g_Ay*T1_acc; 285 | a1_g_Bz = G1(3) - a1_g_Az*T1_acc; 286 | 287 | % gyroscope parameters for sparkfun 288 | w1_b_Ax = (w1b_Bx - w1_Bx) / (T2_gyro - T1_gyro); 289 | w1_b_Ay = (w1b_By - w1_By) / (T2_gyro - T1_gyro); 290 | w1_b_Az = (w1b_Bz - w1_Bz) / (T2_gyro - T1_gyro); 291 | 292 | w1_b_Bx = w1_Bx - w1_b_Ax*T1_gyro; 293 | w1_b_By = w1_By - w1_b_Ay*T1_gyro; 294 | w1_b_Bz = w1_Bz - w1_b_Az*T1_gyro; 295 | 296 | w1_g_Ax = (w1b_Gx - w1_Gx) / (T2_gyro - T1_gyro); 297 | w1_g_Ay = (w1b_Gy - w1_Gy) / (T2_gyro - T1_gyro); 298 | w1_g_Az = (w1b_Gz - w1_Gz) / (T2_gyro - T1_gyro); 299 | 300 | w1_g_Bx = w1_Gx - w1_g_Ax*T1_gyro; 301 | w1_g_By = w1_Gy - w1_g_Ay*T1_gyro; 302 | w1_g_Bz = w1_Gz - w1_g_Az*T1_gyro; 303 | 304 | 305 | % print output for matlab 306 | disp(' '); 307 | disp('Simple calibration model for gyroscopes and accelerometers'); 308 | disp('model: calibrated = (measurement - bias)/gain'); 309 | disp(' '); 310 | disp('Sparkfun 6DOF digital:'); 311 | disp(['w1_bias_xyz = [' num2str(w1_Bx, '%.8f') ... 312 | ', ' num2str(w1_By, '%.8f') ', ' num2str(w1_Bz, '%.8f') '];']); 313 | disp(['w1_gain_xyz = [' num2str(w1_Gx, '%.8f') ... 314 | ', ' num2str(w1_Gy, '%.8f') ', ' num2str(w1_Gz, '%.8f') '];']); 315 | disp(['a1_bias_xyz = [' num2str(B1(1), '%.8f') ... 316 | ', ' num2str(B1(2), '%.8f') ', ' num2str(B1(3), '%.8f') '];']); 317 | disp(['a1_gain_xyz = [' num2str(G1(1), '%.8f') ... 318 | ', ' num2str(G1(2), '%.8f') ', ' num2str(G1(3), '%.8f') '];']); 319 | disp(' '); 320 | disp('InertiaLink IMU:'); 321 | disp(['w2_bias_xyz = [' num2str(w2_Bx, '%.8f') ... 322 | ', ' num2str(w2_By, '%.8f') ', ' num2str(w2_Bz, '%.8f') '];']); 323 | disp(['w2_gain_xyz = [' num2str(w2_Gx, '%.8f') ... 324 | ', ' num2str(w2_Gy, '%.8f') ', ' num2str(w2_Gz, '%.8f') '];']); 325 | disp(['a2_bias_xyz = [' num2str(B2(1), '%.8f') ... 326 | ', ' num2str(B2(2), '%.8f') ', ' num2str(B2(3), '%.8f') '];']); 327 | disp(['a2_gain_xyz = [' num2str(G2(1), '%.8f') ... 328 | ', ' num2str(G2(2), '%.8f') ', ' num2str(G2(3), '%.8f') '];']); 329 | disp(' '); 330 | 331 | 332 | disp(' '); 333 | disp('Temperature based calibration model for gyroscopes and accelerometers'); 334 | disp('model: calibrated = (measurement - bias(T))/gain(T)'); 335 | disp(' bias(T) = bias_A*T + bias_B'); 336 | disp(' gain(T) = gain_A*T + gain_B'); 337 | disp(' '); 338 | disp('Sparkfun 6DOF digital (only SparkFun has temperature data):'); 339 | disp(['w1_bias_a_xyz = [' num2str(w1_b_Ax, '%.8f') ... 340 | ', ' num2str(w1_b_Ay, '%.8f') ', ' num2str(w1_b_Az, '%.8f') '];']); 341 | disp(['w1_bias_b_xyz = [' num2str(w1_b_Bx, '%.8f') ... 342 | ', ' num2str(w1_b_By, '%.8f') ', ' num2str(w1_b_Bz, '%.8f') '];']); 343 | 344 | disp(['w1_gain_a_xyz = [' num2str(w1_g_Ax, '%.8f') ... 345 | ', ' num2str(w1_g_Ay, '%.8f') ', ' num2str(w1_g_Az, '%.8f') '];']); 346 | disp(['w1_gain_b_xyz = [' num2str(w1_g_Bx, '%.8f') ... 347 | ', ' num2str(w1_g_By, '%.8f') ', ' num2str(w1_g_Bz, '%.8f') '];']); 348 | 349 | disp(['a1_bias_a_xyz = [' num2str(a1_b_Ax, '%.8f') ... 350 | ', ' num2str(a1_b_Ay, '%.8f') ', ' num2str(a1_b_Az, '%.8f') '];']); 351 | disp(['a1_bias_b_xyz = [' num2str(a1_b_Bx, '%.8f') ... 352 | ', ' num2str(a1_b_By, '%.8f') ', ' num2str(a1_b_Bz, '%.8f') '];']); 353 | 354 | disp(['a1_gain_a_xyz = [' num2str(a1_g_Ax, '%.8f') ... 355 | ', ' num2str(a1_g_Ay, '%.8f') ', ' num2str(a1_g_Az, '%.8f') '];']); 356 | disp(['a1_gain_b_xyz = [' num2str(a1_g_Bx, '%.8f') ... 357 | ', ' num2str(a1_g_By, '%.8f') ', ' num2str(a1_g_Bz, '%.8f') '];']); 358 | disp(' '); 359 | 360 | 361 | % perform calibration 362 | if (true) 363 | %temperature calibration (using filtered temperature to minimize noise) 364 | T_padded = [data.SparkFun6DOF.temperature(1)*ones(250,1); data.SparkFun6DOF.temperature; ... 365 | data.SparkFun6DOF.temperature(end)*ones(250,1)]; 366 | T = filter2(ones(501,1)/501, T_padded, 'valid'); 367 | acc1_x = (data.SparkFun6DOF.acc_x - (a1_b_Ax*T + a1_b_Bx)) ./ (a1_g_Ax*T + a1_g_Bx); 368 | acc1_y = (data.SparkFun6DOF.acc_y - (a1_b_Ay*T + a1_b_By)) ./ (a1_g_Ay*T + a1_g_By); 369 | acc1_z = (data.SparkFun6DOF.acc_z - (a1_b_Az*T + a1_b_Bz)) ./ (a1_g_Az*T + a1_g_Bz); 370 | 371 | w1_x = (data.SparkFun6DOF.w_x - (w1_b_Ax*T + w1_b_Bx)) ./ (w1_g_Ax*T + w1_g_Bx); 372 | w1_y = (data.SparkFun6DOF.w_y - (w1_b_Ay*T + w1_b_By)) ./ (w1_g_Ay*T + w1_g_By); 373 | w1_z = (data.SparkFun6DOF.w_z - (w1_b_Az*T + w1_b_Bz)) ./ (w1_g_Az*T + w1_g_Bz); 374 | 375 | if (false) 376 | % Unit test 377 | acc1_x_ref = (data.SparkFun6DOF.acc_x - B1(1)) / G1(1); 378 | acc1_y_ref = (data.SparkFun6DOF.acc_y - B1(2)) / G1(2); 379 | acc1_z_ref = (data.SparkFun6DOF.acc_z - B1(3)) / G1(3); 380 | 381 | w1_x_ref = (data.SparkFun6DOF.w_x - w1_Bx) / w1_Gx; 382 | w1_y_ref = (data.SparkFun6DOF.w_y - w1_By) / w1_Gy; 383 | w1_z_ref = (data.SparkFun6DOF.w_z - w1_Bz) / w1_Gz; 384 | 385 | notNan = ~isnan(acc1_x); 386 | ax_error = mean(acc1_x(notNan) - acc1_x_ref(notNan)) 387 | ay_error = mean(acc1_y(notNan) - acc1_y_ref(notNan)) 388 | az_error = mean(acc1_z(notNan) - acc1_z_ref(notNan)) 389 | 390 | wx_error = mean(w1_x(notNan) - w1_x_ref(notNan)) 391 | wy_error = mean(w1_y(notNan) - w1_y_ref(notNan)) 392 | wz_error = mean(w1_z(notNan) - w1_z_ref(notNan)) 393 | end 394 | else 395 | %simple calibration 396 | acc1_x = (data.SparkFun6DOF.acc_x - B1(1)) / G1(1); 397 | acc1_y = (data.SparkFun6DOF.acc_y - B1(2)) / G1(2); 398 | acc1_z = (data.SparkFun6DOF.acc_z - B1(3)) / G1(3); 399 | 400 | w1_x = (data.SparkFun6DOF.w_x - w1_Bx) / w1_Gx; 401 | w1_y = (data.SparkFun6DOF.w_y - w1_By) / w1_Gy; 402 | w1_z = (data.SparkFun6DOF.w_z - w1_Bz) / w1_Gz; 403 | end 404 | 405 | acc2_x = (data.InertiaLink.acc_x - B2(1)) / G2(1); 406 | acc2_y = (data.InertiaLink.acc_y - B2(2)) / G2(2); 407 | acc2_z = (data.InertiaLink.acc_z - B2(3)) / G2(3); 408 | 409 | w2_x = (data.InertiaLink.w_x - w2_Bx) / w2_Gx; 410 | w2_y = (data.InertiaLink.w_y - w2_By) / w2_Gy; 411 | w2_z = (data.InertiaLink.w_z - w2_Bz) / w2_Gz; 412 | 413 | 414 | if (false) %debugplotting for calibration 415 | 416 | figure(1); 417 | clf; 418 | subplot(3,1,1); 419 | plot(SFtime, acc1_x, 'c', ILtime, acc2_x, 'b--', 'linewidth', 1); 420 | hold on; 421 | minY = min([min(acc1_x) min(acc2_x)]); 422 | maxY = max([max(acc1_x) max(acc2_x)]); 423 | for i = 1:size(calibTimes,2) 424 | plot([calibTimes(1,i) calibTimes(1,i)], [minY maxY], 'k:'); 425 | plot([calibTimes(2,i) calibTimes(2,i)], [minY maxY], 'k:'); 426 | 427 | plot([calibTimes2(1,i) calibTimes2(1,i)], [minY maxY], 'k:'); 428 | plot([calibTimes2(2,i) calibTimes2(2,i)], [minY maxY], 'k:'); 429 | end 430 | for i = 1:size(angleCalibTimes,2) 431 | plot([angleCalibTimes(1,i) angleCalibTimes(1,i)], [minY maxY], 'r:'); 432 | plot([angleCalibTimes(end,i) angleCalibTimes(end,i)], [minY maxY], 'r:'); 433 | 434 | plot([angleCalibTimes2(1,i) angleCalibTimes2(1,i)], [minY maxY], 'r:'); 435 | plot([angleCalibTimes2(end,i) angleCalibTimes2(end,i)], [minY maxY], 'r:'); 436 | end 437 | axis([minCalibTime, maxCalibTime, minY, maxY]) 438 | ylabel('x'); 439 | 440 | subplot(3,1,2); 441 | plot(SFtime, acc1_y, 'c', ILtime, acc2_y, 'b--', 'linewidth', 1); 442 | hold on; 443 | minY = min([min(acc1_y) min(acc2_y)]); 444 | maxY = max([max(acc1_y) max(acc2_y)]); 445 | for i = 1:size(calibTimes,2) 446 | plot([calibTimes(1,i) calibTimes(1,i)], [minY maxY], 'k:'); 447 | plot([calibTimes(2,i) calibTimes(2,i)], [minY maxY], 'k:'); 448 | 449 | plot([calibTimes2(1,i) calibTimes2(1,i)], [minY maxY], 'k:'); 450 | plot([calibTimes2(2,i) calibTimes2(2,i)], [minY maxY], 'k:'); 451 | end 452 | for i = 1:size(angleCalibTimes,2) 453 | plot([angleCalibTimes(1,i) angleCalibTimes(1,i)], [minY maxY], 'r:'); 454 | plot([angleCalibTimes(end,i) angleCalibTimes(end,i)], [minY maxY], 'r:'); 455 | 456 | plot([angleCalibTimes2(1,i) angleCalibTimes2(1,i)], [minY maxY], 'r:'); 457 | plot([angleCalibTimes2(end,i) angleCalibTimes2(end,i)], [minY maxY], 'r:'); 458 | end 459 | axis([minCalibTime, maxCalibTime, minY, maxY]) 460 | ylabel('y'); 461 | 462 | subplot(3,1,3); 463 | plot(SFtime, acc1_z, 'c', ILtime, acc2_z, 'b--', 'linewidth', 1); 464 | hold on; 465 | minY = min([min(acc1_z) min(acc2_z)]); 466 | maxY = max([max(acc1_z) max(acc2_z)]); 467 | for i = 1:size(calibTimes,2) 468 | plot([calibTimes(1,i) calibTimes(1,i)], [minY maxY], 'k:'); 469 | plot([calibTimes(2,i) calibTimes(2,i)], [minY maxY], 'k:'); 470 | 471 | plot([calibTimes2(1,i) calibTimes2(1,i)], [minY maxY], 'k:'); 472 | plot([calibTimes2(2,i) calibTimes2(2,i)], [minY maxY], 'k:'); 473 | end 474 | for i = 1:size(angleCalibTimes,2) 475 | plot([angleCalibTimes(1,i) angleCalibTimes(1,i)], [minY maxY], 'r:'); 476 | plot([angleCalibTimes(end,i) angleCalibTimes(end,i)], [minY maxY], 'r:'); 477 | 478 | plot([angleCalibTimes2(1,i) angleCalibTimes2(1,i)], [minY maxY], 'r:'); 479 | plot([angleCalibTimes2(end,i) angleCalibTimes2(end,i)], [minY maxY], 'r:'); 480 | end 481 | ylabel('z'); 482 | axis([minCalibTime, maxCalibTime, minY, maxY]) 483 | pause(0.2); 484 | 485 | 486 | figure(2); 487 | clf; 488 | subplot(3,1,1); 489 | plot(Ktime, gyro(:,1), 'k', SFtime-SFdelay, w1_x, 'c', ILtime-ILdelay, w2_x, 'b--', 'linewidth', 1); 490 | hold on; 491 | minY = min([min(w1_x) min(w2_x)]); 492 | maxY = max([max(w1_x) max(w2_x)]); 493 | for i = 1:size(calibTimes,2) 494 | plot([calibTimes(1,i) calibTimes(1,i)], [minY maxY], 'k:'); 495 | plot([calibTimes(2,i) calibTimes(2,i)], [minY maxY], 'k:'); 496 | end 497 | for i = 1:size(angleCalibTimes,2) 498 | plot([angleCalibTimes(1,i) angleCalibTimes(1,i)], [minY maxY], 'r:'); 499 | plot([angleCalibTimes(end,i) angleCalibTimes(end,i)], [minY maxY], 'r:'); 500 | end 501 | axis([minTime, maxTime, minY, maxY]) 502 | ylabel('x'); 503 | 504 | subplot(3,1,2); 505 | plot(Ktime, gyro(:,2), 'k', SFtime-SFdelay, w1_y, 'c', ILtime-ILdelay, w2_y, 'b--', 'linewidth', 1); 506 | hold on; 507 | minY = min([min(w1_y) min(w2_y)]); 508 | maxY = max([max(w1_y) max(w2_y)]); 509 | for i = 1:size(calibTimes,2) 510 | plot([calibTimes(1,i) calibTimes(1,i)], [minY maxY], 'k:'); 511 | plot([calibTimes(2,i) calibTimes(2,i)], [minY maxY], 'k:'); 512 | end 513 | for i = 1:size(angleCalibTimes,2) 514 | plot([angleCalibTimes(1,i) angleCalibTimes(1,i)], [minY maxY], 'r:'); 515 | plot([angleCalibTimes(end,i) angleCalibTimes(end,i)], [minY maxY], 'r:'); 516 | end 517 | axis([minTime, maxTime, minY, maxY]) 518 | ylabel('y'); 519 | 520 | subplot(3,1,3); 521 | plot(Ktime, gyro(:,3), 'k', SFtime-SFdelay, w1_z, 'c', ILtime-ILdelay, w2_z, 'b--', 'linewidth', 1); 522 | hold on; 523 | minY = min([min(w1_z) min(w2_z)]); 524 | maxY = max([max(w1_z) max(w2_z)]); 525 | for i = 1:size(calibTimes,2) 526 | plot([calibTimes(1,i) calibTimes(1,i)], [minY maxY], 'k:'); 527 | plot([calibTimes(2,i) calibTimes(2,i)], [minY maxY], 'k:'); 528 | end 529 | for i = 1:size(angleCalibTimes,2) 530 | plot([angleCalibTimes(1,i) angleCalibTimes(1,i)], [minY maxY], 'r:'); 531 | plot([angleCalibTimes(end,i) angleCalibTimes(end,i)], [minY maxY], 'r:'); 532 | end 533 | ylabel('z'); 534 | axis([minTime, maxTime, minY, maxY]) 535 | pause(0.2); 536 | 537 | if (false) %test calibration parameters (run calibration with corrected data) 538 | w_x_error_SF = w1_x(SFrange) - w_x_in_SF; 539 | w_y_error_SF = w1_y(SFrange) - w_y_in_SF; 540 | w_z_error_SF = w1_z(SFrange) - w_z_in_SF; 541 | 542 | w_x_error_IL = w2_x(ILrange) - w_x_in_IL; 543 | w_y_error_IL = w2_y(ILrange) - w_y_in_IL; 544 | w_z_error_IL = w2_z(ILrange) - w_z_in_IL; 545 | 546 | [w1_Gx, w1_Bx] = fitRegressionLine(w_x_in_SF(x_filt_SF), w_x_in_SF(x_filt_SF)+w_x_error_SF(x_filt_SF)); 547 | [w2_Gx, w2_Bx] = fitRegressionLine(w_x_in_IL(x_filt_IL), w_x_in_IL(x_filt_IL)+w_x_error_IL(x_filt_IL)); 548 | [w1_Gy, w1_By] = fitRegressionLine(w_y_in_SF(y_filt_SF), w_y_in_SF(y_filt_SF)+w_y_error_SF(y_filt_SF)); 549 | [w2_Gy, w2_By] = fitRegressionLine(w_y_in_IL(y_filt_IL), w_y_in_IL(y_filt_IL)+w_y_error_IL(y_filt_IL)); 550 | [w1_Gz, w1_Bz] = fitRegressionLine(w_z_in_SF(z_filt_SF), w_z_in_SF(z_filt_SF)+w_z_error_SF(z_filt_SF)); 551 | [w2_Gz, w2_Bz] = fitRegressionLine(w_z_in_IL(z_filt_IL), w_z_in_IL(z_filt_IL)+w_z_error_IL(z_filt_IL)); 552 | end 553 | 554 | figure(3); 555 | clf; 556 | subplot(3,1,1); 557 | minY = -pi; 558 | maxY = pi; 559 | plot(w_x_in_SF(x_filt_SF), w_x_error_SF(x_filt_SF), 'b.', w_x_in_IL(x_filt_IL), w_x_error_IL(x_filt_IL), 'm.', 'linewidth', 1, 'markersize', 3); 560 | hold on; 561 | x_vals = [min(w_x_in_SF(x_filt_SF)), max(w_x_in_SF(x_filt_SF))]; 562 | plot(x_vals, (w1_Gx-1)*x_vals + w1_Bx, 'b', x_vals, (w2_Gx-1)*x_vals + w2_Bx, 'm', 'linewidth', 1); 563 | ylabel('w_x error (rad/s)'); 564 | xlabel('reference w_x (rad/s)'); 565 | 566 | 567 | subplot(3,1,2); 568 | plot(w_y_in_SF(y_filt_SF), w_y_error_SF(y_filt_SF), 'b.', w_y_in_IL(y_filt_IL), w_y_error_IL(y_filt_IL), 'm.', 'linewidth', 1, 'markersize', 3); 569 | hold on; 570 | y_vals = [min(w_y_in_SF(y_filt_SF)), max(w_y_in_SF(y_filt_SF))]; 571 | plot(y_vals, (w1_Gy-1)*y_vals + w1_By, 'b', y_vals, (w2_Gy-1)*y_vals + w2_By, 'm', 'linewidth', 1); 572 | ylabel('w_y error (rad/s)'); 573 | xlabel('reference w_y (rad/s)'); 574 | %ylabel('w_y (rad/s)'); 575 | 576 | subplot(3,1,3); 577 | plot(w_z_in_SF(z_filt_SF), w_z_error_SF(z_filt_SF), 'b.', w_z_in_IL(z_filt_IL), w_z_error_IL(z_filt_IL), 'm.', 'linewidth', 1, 'markersize', 3); 578 | hold on; 579 | z_vals = [min(w_z_in_SF(z_filt_SF)), max(w_z_in_SF(z_filt_SF))]; 580 | plot(z_vals, (w1_Gz-1)*z_vals + w1_Bz, 'b', z_vals, (w2_Gz-1)*z_vals + w2_Bz, 'm', 'linewidth', 1); 581 | ylabel('w_z error (rad/s)'); 582 | xlabel('reference w_z (rad/s)'); 583 | %ylabel('w_z (rad/s)'); 584 | 585 | pause(0.2); 586 | end 587 | -------------------------------------------------------------------------------- /legendLocation.m: -------------------------------------------------------------------------------- 1 | %============================================================================ 2 | % Copyright (C) 2014, Heikki Hyyti 3 | % 4 | % Permission is hereby granted, free of charge, to any person obtaining a 5 | % copy of this software and associated documentation files (the "Software"), 6 | % to deal in the Software without restriction, including without limitation 7 | % the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | % and/or sell copies of the Software, and to permit persons to whom the 9 | % Software is furnished to do so, subject to the following conditions: 10 | % 11 | % The above copyright notice and this permission notice shall be included in 12 | % all copies or substantial portions of the Software. 13 | % 14 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | % THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | % FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | % DEALINGS IN THE SOFTWARE. 21 | %============================================================================ 22 | 23 | %set legend location to NorthWest, NorthEast, SouthWest or SouthEast where 24 | %it least conflicts the data in plot. data is given in a matrix of 25 | %different data sets in different columns. equally sampled data is assumed 26 | %from first to last item from left to right 27 | 28 | function location = legendLocation(data) 29 | dataLen = size(data,1); 30 | dataMaxVal = max(data(:)); 31 | dataMinVal = min(data(:)); 32 | first_part = data(1:ceil(dataLen/4),:); 33 | firstMaxVal = max(first_part(:)); 34 | firstMinVal = min(first_part(:)); 35 | last_part = data(floor(3*dataLen/4):end,:); 36 | lastMaxVal = max(last_part(:)); 37 | lastMinVal = min(last_part(:)); 38 | 39 | selectLabels = {'NorthWest', 'NorthEast', 'SouthWest', 'SouthEast'}; 40 | selectVals = zeros(4,1); 41 | selectVals(1) = dataMaxVal - firstMaxVal; 42 | selectVals(2) = dataMaxVal - lastMaxVal; 43 | selectVals(3) = firstMinVal - dataMinVal; 44 | selectVals(4) = lastMinVal - dataMinVal; 45 | 46 | [~,idx] = max(selectVals); 47 | location = selectLabels{idx}; 48 | end -------------------------------------------------------------------------------- /plotIMUsWithKuka.m: -------------------------------------------------------------------------------- 1 | %============================================================================ 2 | % Copyright (C) 2015, Heikki Hyyti 3 | % 4 | % Permission is hereby granted, free of charge, to any person obtaining a 5 | % copy of this software and associated documentation files (the "Software"), 6 | % to deal in the Software without restriction, including without limitation 7 | % the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | % and/or sell copies of the Software, and to permit persons to whom the 9 | % Software is furnished to do so, subject to the following conditions: 10 | % 11 | % The above copyright notice and this permission notice shall be included in 12 | % all copies or substantial portions of the Software. 13 | % 14 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | % THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | % FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | % DEALINGS IN THE SOFTWARE. 21 | %============================================================================ 22 | 23 | disp(' '); 24 | disp('If you use the algorithm in any scientific context, please cite:'); 25 | disp('Heikki Hyyti and Arto Visala, "A DCM Based Attitude Estimation Algorithm'); 26 | disp('for Low-Cost MEMS IMUs," International Journal of Navigation and Observation,'); 27 | disp('vol. 2015, Article ID 503814, 18 pages, 2015. http://dx.doi.org/10.1155/2015/503814'); 28 | disp(' '); 29 | 30 | clear all; 31 | close all; 32 | pause(0.1); 33 | 34 | %% Configuration 35 | 36 | % use c/compile.m before setting this to true (tested only with linux) 37 | use_C_versions = false; 38 | 39 | % fetch GPL licensed algortihms in matlab and C from here before use: 40 | % http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ 41 | % Add the C code to c folder and matlab functions in this folder 42 | % (please add also the quaternion library used by Madgwick). 43 | % I apologize this mess. The Madgwick's GPS licenced code is separated from 44 | % this MIT licenced product to avoid infecting this code with GPL licence. 45 | use_comparisonAlgorithms = false; 46 | 47 | % Add bias to calibrated gyro values (same for each axis) 48 | addedGyroBias = 0; 49 | 50 | % load measurement data 51 | data = load('allData.mat'); 52 | 53 | if (~exist('figures', 'dir')); mkdir('figures'); end; 54 | 55 | ILtime = data.InertiaLink.tv_sec + (data.InertiaLink.tv_msec/1000); 56 | SFtime = data.SparkFun6DOF.tv_sec + (data.SparkFun6DOF.tv_usec / 1000000); 57 | Ktime = data.Kuka.tv_sec + (data.Kuka.tv_usec/1000000); 58 | 59 | firstCommonTime = max([ILtime(1), SFtime(1), Ktime(1)]); 60 | lastCommonTime = min([ILtime(end), SFtime(end), Ktime(end)]); 61 | commonDuration = lastCommonTime - firstCommonTime; 62 | 63 | ILtime = ILtime - firstCommonTime; 64 | SFtime = SFtime - firstCommonTime; 65 | Ktime = Ktime - firstCommonTime; 66 | 67 | ILdeltaTime = [0; ILtime(2:end) - ILtime(1:end-1)]; 68 | SFdeltaTime = [0; SFtime(2:end) - SFtime(1:end-1)]; 69 | KdeltaTime = [0; Ktime(2:end) - Ktime(1:end-1)]; 70 | 71 | %start yaw from zero at time 468s (start of tests after calibration sequences in example data set) 72 | StartIdx = find(ILtime > 464, 1, 'first'); 73 | StopIdx = find(ILtime > 774, 1, 'first'); 74 | time = ILtime(StartIdx:StopIdx); 75 | 76 | %getting reference trajectory 77 | R11 = data.Kuka.data_msrCartPos01; 78 | R12 = data.Kuka.data_msrCartPos02; 79 | R13 = data.Kuka.data_msrCartPos03; 80 | x = data.Kuka.data_msrCartPos04; 81 | R21 = data.Kuka.data_msrCartPos05; 82 | R22 = data.Kuka.data_msrCartPos06; 83 | R23 = data.Kuka.data_msrCartPos07; 84 | y = data.Kuka.data_msrCartPos08; 85 | R31 = data.Kuka.data_msrCartPos09; 86 | R32 = data.Kuka.data_msrCartPos10; 87 | R33 = data.Kuka.data_msrCartPos11; 88 | z = data.Kuka.data_msrCartPos12; 89 | 90 | [yaw, pitch, roll] = yawpitchroll(R11, R21, R31, R32, R33); 91 | yaw = un_modulo(yaw, 2*pi); %remove 2pi jumps and make yaw continuous 92 | yaw = yaw - yaw(1); %starting yaw from zero (same as IMU estimates) 93 | 94 | vel_x = [0; x(2:end)-x(1:end-1)] ./ KdeltaTime; 95 | vel_y = [0; y(2:end)-y(1:end-1)] ./ KdeltaTime; 96 | vel_z = [0; z(2:end)-z(1:end-1)] ./ KdeltaTime; 97 | 98 | % compute reference angular velocities using the KUKA robot 99 | % (derivate between positions) 100 | gyro = zeros(length(R11),3); 101 | dt = median(Ktime(2:end)-Ktime(1:end-1)); % the most frequently used discretation time 102 | for i = 2:length(R11) 103 | R_last = [R11(i-1), R12(i-1), R13(i-1); ... 104 | R21(i-1), R22(i-1), R23(i-1); ... 105 | R31(i-1), R32(i-1), R33(i-1)]; 106 | R_cur = [R11(i), R12(i), R13(i); ... 107 | R21(i), R22(i), R23(i); ... 108 | R31(i), R32(i), R33(i)]; 109 | 110 | Wh = (R_last'*R_cur - eye(3)); 111 | W = 1/dt * Wh; 112 | gyro(i,1) = (W(3,2) - W(2,3))/2; 113 | gyro(i,2) = (W(1,3) - W(3,1))/2; 114 | gyro(i,3) = (W(2,1) - W(1,2))/2; 115 | end 116 | 117 | % Resample reference data of KUKA robot to InertiaLink time 118 | warning('off','interpolation:interpolation:noextrap'); 119 | yaw_in_IL = resample(timeseries(yaw, Ktime),time); 120 | yaw_in_IL = yaw_in_IL.Data; 121 | yaw_in_IL = yaw_in_IL - yaw_in_IL(1); %starting yaw from zero (same as IMU estimates) 122 | pitch_in_IL = resample(timeseries(pitch, Ktime),time); 123 | pitch_in_IL = pitch_in_IL.Data; 124 | roll_in_IL = resample(timeseries(roll, Ktime),time); 125 | roll_in_IL = roll_in_IL.Data; 126 | 127 | %computing ypr for InertiaLink internal estimates (Rotation matrix R = -M') 128 | [yaw_IL, pitch_IL, roll_IL] = yawpitchroll(-data.InertiaLink.M11, ... 129 | data.InertiaLink.M12, -data.InertiaLink.M13, -data.InertiaLink.M23, ... 130 | -data.InertiaLink.M33); 131 | yaw_IL = un_modulo(yaw_IL, 2*pi); % remove 2pi jumps and make yaw continuous 132 | yaw_IL = yaw_IL - yaw_IL(1); % starting yaw from zero (same as IMU estimates) 133 | 134 | %% calibrate IMU data 135 | imusWithKukaCalibration; 136 | 137 | acc1 = [acc1_x, acc1_y, acc1_z]; 138 | acc2 = [acc2_x, acc2_y, acc2_z]; 139 | 140 | gyro1 = [w1_x, w1_y, w1_z]; 141 | gyro2 = [w2_x, w2_y, w2_z]; 142 | 143 | % add constant bias for testing purposes (if addedGyroBias is set above) 144 | gyro1 = gyro1 + addedGyroBias*ones(size(gyro1)); 145 | gyro2 = gyro2 + addedGyroBias*ones(size(gyro2)); 146 | 147 | %% DCM IMU results for both imu datas 148 | tic(); 149 | if (use_C_versions) 150 | [x_hist1, ypr_hist1, a_hist1, P_diag_hist1] = ... 151 | DCM_IMU_uC(gyro1, acc1, SFdeltaTime); 152 | else 153 | IMU_DCM1 = DCM_IMU(); 154 | x_hist1 = zeros(length(SFtime), 6); 155 | ypr_hist1 = zeros(length(SFtime), 3); 156 | P_diag_hist1 = zeros(length(SFtime), 6); 157 | for t = 1:length(SFtime) 158 | IMU_DCM1.UpdateIMU(gyro1(t,:), acc1(t,:), SFdeltaTime(t)); % gyroscope units must be radians 159 | x_hist1(t, :) = IMU_DCM1.state'; 160 | ypr_hist1(t, :) = [IMU_DCM1.yaw, IMU_DCM1.pitch, IMU_DCM1.roll]; 161 | P_diag_hist1(t, :) = diag(IMU_DCM1.P)'; 162 | end 163 | end 164 | tDCM1 = toc() 165 | 166 | tic(); 167 | if (use_C_versions) 168 | [x_hist2, ypr_hist2, a_hist2, P_diag_hist2] = ... 169 | DCM_IMU_uC(gyro2, acc2, ILdeltaTime); 170 | else 171 | IMU_DCM2 = DCM_IMU(); 172 | x_hist2 = zeros(length(ILtime), 6); 173 | ypr_hist2 = zeros(length(ILtime), 3); 174 | P_diag_hist2 = zeros(length(ILtime), 6); 175 | for t = 1:length(ILtime) 176 | IMU_DCM2.UpdateIMU(gyro2(t,:), acc2(t,:), ILdeltaTime(t)); % gyroscope units must be radians 177 | x_hist2(t, :) = IMU_DCM2.state'; 178 | ypr_hist2(t, :) = [IMU_DCM2.yaw, IMU_DCM2.pitch, IMU_DCM2.roll]; 179 | P_diag_hist2(t, :) = diag(IMU_DCM2.P)'; 180 | end 181 | end 182 | tDCM2 = toc() 183 | 184 | %% computing Madgwick and Mahony imu algorithms for comparison 185 | if (use_comparisonAlgorithms) 186 | addpath('quaternion_library'); % include quaternion library 187 | 188 | tic(); 189 | if (use_C_versions) 190 | quat_mad_B = Madgwick_IMU_C(gyro1, acc1); 191 | else 192 | IMU_mad_B = MadgwickAHRS('SamplePeriod', 1/150, 'Beta', 0.1); 193 | quat_mad_B = zeros(length(SFtime), 4); 194 | for t = 1:length(quat_mad_B) 195 | IMU_mad_B.UpdateIMU(gyro1(t,:), acc1(t,:)); % gyroscope units must be radians 196 | quat_mad_B(t, :) = IMU_mad_B.Quaternion; 197 | end 198 | end 199 | tIMU_mad_B = toc() 200 | 201 | tic(); 202 | if (use_C_versions) 203 | quat_mah_B = Mahony_IMU_C(gyro1, acc1); 204 | else 205 | IMU_mah_B = MahonyAHRS('SamplePeriod', 1/150, 'Kp', 0.5); 206 | quat_mah_B = zeros(length(SFtime), 4); 207 | for t = 1:length(quat_mah_B) 208 | IMU_mah_B.UpdateIMU(gyro1(t,:), acc1(t,:)); % gyroscope units must be radians 209 | quat_mah_B(t, :) = IMU_mah_B.Quaternion; 210 | end 211 | end 212 | 213 | tIMU_mah_B = toc() 214 | 215 | tic(); 216 | if (use_C_versions) 217 | quat_mad_A = Madgwick_IMU_C(gyro2, acc2); 218 | else 219 | IMU_mad_A = MadgwickAHRS('SamplePeriod', 1/150, 'Beta', 0.1); 220 | quat_mad_A = zeros(length(ILtime), 4); 221 | for t = 1:length(quat_mad_A) 222 | IMU_mad_A.UpdateIMU(gyro2(t,:), acc2(t,:)); % gyroscope units must be radians 223 | quat_mad_A(t, :) = IMU_mad_A.Quaternion; 224 | end 225 | end 226 | tIMU_mad_A = toc() 227 | 228 | tic(); 229 | if (use_C_versions) 230 | quat_mah_A = Mahony_IMU_C(gyro2, acc2); 231 | else 232 | IMU_mah_A = MahonyAHRS('SamplePeriod', 1/150, 'Kp', 0.5); 233 | quat_mah_A = zeros(length(ILtime), 4); 234 | for t = 1:length(quat_mah_A) 235 | IMU_mah_A.UpdateIMU(gyro2(t,:), acc2(t,:)); % gyroscope units must be radians 236 | quat_mah_A(t, :) = IMU_mah_A.Quaternion; 237 | end 238 | end 239 | tIMU_mah_A = toc() 240 | 241 | % Plot algorithm output as Euler angles 242 | % The first and third Euler angles in the sequence (phi and psi) become 243 | % unreliable when the middle angles of the sequence (theta) approaches ~90 244 | % degrees. This problem commonly referred to as Gimbal Lock. 245 | % See: http://en.wikipedia.org/wiki/Gimbal_lock 246 | 247 | % use conjugate for sensor frame relative to Earth and convert to degrees. 248 | euler_mad_B = quatern2euler(quaternConj(quat_mad_B)) * (180/pi); 249 | euler_mah_B = quatern2euler(quaternConj(quat_mah_B)) * (180/pi); 250 | euler_mad_A = quatern2euler(quaternConj(quat_mad_A)) * (180/pi); 251 | euler_mah_A = quatern2euler(quaternConj(quat_mah_A)) * (180/pi); 252 | else 253 | euler_mad_B = nan(length(gyro1),3); 254 | euler_mah_B = nan(length(gyro1),3); 255 | euler_mad_A = nan(length(gyro2),3); 256 | euler_mah_A = nan(length(gyro2),3); 257 | 258 | disp('To get reference algorithms, download their implementations from here:'); 259 | disp('http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/'); 260 | end 261 | 262 | 263 | %% resampling & remove 2pi jumps 264 | warning('off','interpolation:interpolation:noextrap'); 265 | 266 | %resample SparkFun data to InertiaLink time 267 | yaw_sf_in_IL = resample(timeseries(un_modulo(ypr_hist1(:,1),2*pi), SFtime),time); 268 | yaw_sf_in_IL = yaw_sf_in_IL.Data*180/pi; 269 | yaw_sf_in_IL = yaw_sf_in_IL - yaw_sf_in_IL(1); %starting yaw from zero (same as IMU estimates) 270 | pitch_sf_in_IL = resample(timeseries(ypr_hist1(:,2), SFtime),time); 271 | pitch_sf_in_IL = pitch_sf_in_IL.Data*180/pi; 272 | roll_sf_in_IL = resample(timeseries(ypr_hist1(:,3), SFtime),time); 273 | roll_sf_in_IL = roll_sf_in_IL.Data*180/pi; 274 | 275 | 276 | % start yaw from zero at the beginning of test sequence 277 | yaw_IL = yaw_IL - yaw_IL(StartIdx); 278 | 279 | %yaw pitch roll errors (only InertiaLink data) 280 | ypr_hist2(:,1) = un_modulo(ypr_hist2(:,1), 2*pi); %remove 2pi jumps and make continuous 281 | ypr_hist2(:,1) = ypr_hist2(:,1) - ypr_hist2(StartIdx,1); %DCM 282 | 283 | euler_mad_B(:,3) = un_modulo(euler_mad_B(:,3), 360); %remove 2pi jumps and make continuous 284 | r_data = resample(timeseries(euler_mad_B(:,1), SFtime),ILtime); 285 | p_data = resample(timeseries(euler_mad_B(:,2), SFtime),ILtime); 286 | y_data = resample(timeseries(euler_mad_B(:,3), SFtime),ILtime); 287 | euler_mad_B = [r_data.data, p_data.data, y_data.data]; 288 | euler_mad_B(:,3) = euler_mad_B(:,3) - euler_mad_B(StartIdx,3); %Madgwick 289 | 290 | euler_mah_B(:,3) = un_modulo(euler_mah_B(:,3), 360); %remove 2pi jumps and make continuous 291 | r_data = resample(timeseries(euler_mah_B(:,1), SFtime),ILtime); 292 | p_data = resample(timeseries(euler_mah_B(:,2), SFtime),ILtime); 293 | y_data = resample(timeseries(euler_mah_B(:,3), SFtime),ILtime); 294 | euler_mah_B = [r_data.data, p_data.data, y_data.data]; 295 | euler_mah_B(:,3) = euler_mah_B(:,3) - euler_mah_B(StartIdx,3); %Mahony 296 | 297 | euler_mad_A(:,3) = un_modulo(euler_mad_A(:,3), 360); %remove 2pi jumps and make continuous 298 | euler_mad_A(:,3) = euler_mad_A(:,3) - euler_mad_A(StartIdx,3); %Madgwick 299 | 300 | euler_mah_A(:,3) = un_modulo(euler_mah_A(:,3), 360); %remove 2pi jumps and make continuous 301 | euler_mah_A(:,3) = euler_mah_A(:,3) - euler_mah_A(StartIdx,3); %Mahony 302 | 303 | warning('on','interpolation:interpolation:noextrap'); 304 | 305 | %% plotting 306 | markerSize = 8; %pl 307 | 308 | figPos = [105 105 645 660]; 309 | 310 | plotPos = [0.1 0.15 0.70 0.70]; 311 | 312 | plot21Pos = [0.1 0.53 0.86 0.40]; 313 | plot22Pos = plot21Pos + [0 -0.47 0 0]; 314 | 315 | plot1Pos = [0.1 0.69 0.86 0.25]; 316 | plot2Pos = plot1Pos + [0 -0.31 0 0]; 317 | plot3Pos = plot2Pos + [0 -0.31 0 0]; 318 | 319 | plot41Pos = [0.1 0.75 0.86 0.19]; 320 | plot42Pos = plot41Pos + [0 -0.23 0 0]; 321 | plot43Pos = plot42Pos + [0 -0.23 0 0]; 322 | plot44Pos = plot43Pos + [0 -0.23 0 0]; 323 | 324 | % Define locations from where tests start and stop (acceleration test and rotation test) 325 | 326 | %ACC-test sequence: Linear motion with different accelerations 327 | accStartIdx = find(time > 468, 1, 'first'); 328 | accStopIdx = find(time > 559, 1, 'first'); 329 | accStartIdx_IL = find(ILtime > 468, 1, 'first'); 330 | accStopIdx_IL = find(ILtime > 559, 1, 'first'); 331 | accStartIdx_SF = find(SFtime > 468, 1, 'first'); 332 | accStopIdx_SF = find(SFtime > 559, 1, 'first'); 333 | accStartIdx_K = find(Ktime > 468, 1, 'first'); 334 | accStopIdx_K = find(Ktime > 559, 1, 'first'); 335 | 336 | %sideways movement at x 337 | accStartIdx_SF_x = find(SFtime > 468.5, 1, 'first'); 338 | accStopIdx_SF_x = find(SFtime > 496.5, 1, 'first'); 339 | %sideways movement at y 340 | accStartIdx_SF_y = find(SFtime > 498, 1, 'first'); 341 | accStopIdx_SF_y = find(SFtime > 526, 1, 'first'); 342 | %up and down movement at z 343 | accStartIdx_SF_z = find(SFtime > 530, 1, 'first'); 344 | accStopIdx_SF_z = find(SFtime > 558, 1, 'first'); 345 | 346 | 347 | %IMU-test sequence: free 6D motion 348 | rotStartIdx = find(time > 564, 1, 'first'); 349 | rotStopIdx = find(time > 770, 1, 'first'); 350 | 351 | %raw accelerations 352 | figIdx = 1; 353 | figure(figIdx); clf; 354 | 355 | arrowOffset = 2; 356 | arrowOffset_y = 0.15; 357 | 358 | subplot(4,1,1); 359 | minY = -1; 360 | maxY = 1; 361 | plot(Ktime, vel_x, 'b', Ktime, vel_y, 'g--', Ktime, vel_z, 'r:', 'LineWidth', 1); 362 | set(gca, 'Position', plot41Pos, 'FontSize', 12, 'FontName', 'Times'); 363 | title('Reference measurement and accelerations during the acceleration test', 'FontSize', 14, 'FontName', 'Times'); 364 | legend('x', 'y', 'z', 'Location', 'NorthEast'); 365 | ylabel('Reference velocity (m/s)', 'FontSize', 12, 'FontName', 'Times'); 366 | axis([Ktime(accStartIdx_K) Ktime(accStopIdx_K) minY maxY]); 367 | hold on; 368 | plot(SFtime([accStartIdx_SF_x accStartIdx_SF_x]), [minY maxY], 'k:', SFtime([accStopIdx_SF_x accStopIdx_SF_x]), [minY maxY], 'k:', 'LineWidth', 1); 369 | plot(SFtime([accStartIdx_SF_y accStartIdx_SF_y]), [minY maxY], 'k:', SFtime([accStopIdx_SF_y accStopIdx_SF_y]), [minY maxY], 'k:', 'LineWidth', 1); 370 | plot(SFtime([accStartIdx_SF_z accStartIdx_SF_z]), [minY maxY], 'k:', SFtime([accStopIdx_SF_z accStopIdx_SF_z]), [minY maxY], 'k:', 'LineWidth', 1); 371 | 372 | 373 | subplot(4,1,2); 374 | plot(ILtime(accStartIdx_IL), acc2_x(accStartIdx_IL), 'b--', ... 375 | SFtime(accStartIdx_SF:accStopIdx_SF), acc1_x(accStartIdx_SF:accStopIdx_SF), 'c', ... 376 | ILtime(accStartIdx_IL:accStopIdx_IL), acc2_x(accStartIdx_IL:accStopIdx_IL), 'b--'); 377 | set(gca, 'Position', plot42Pos, 'FontSize', 12, 'FontName', 'Times'); 378 | %title('Accelerations during the acceleration test', 'FontSize', 14, 'FontName', 'Times'); 379 | legend('A (Inertia-Link)', 'B (SparkFun)', 'Location', legendLocation(acc2_x(accStartIdx_IL:accStopIdx_IL))); 380 | ylabel('acc_x (m/s^2)', 'FontSize', 12, 'FontName', 'Times'); 381 | hold on; 382 | 383 | maxY = max([max(acc1_x(accStartIdx_SF:accStopIdx_SF)), max(acc2_x(accStartIdx_IL:accStopIdx_IL))]); 384 | %maxY = max([1.1*maxY 0.9*maxY]); 385 | minY = min([min(acc1_x(accStartIdx_SF:accStopIdx_SF)), min(acc2_x(accStartIdx_IL:accStopIdx_IL))]); 386 | %minY = min([1.1*minY 0.9*minY]); 387 | plot(SFtime([accStartIdx_SF_x accStartIdx_SF_x]), [minY maxY], 'k:', SFtime([accStopIdx_SF_x accStopIdx_SF_x]), [minY maxY], 'k:', 'LineWidth', 1); 388 | plot(SFtime([accStartIdx_SF_y accStartIdx_SF_y]), [minY maxY], 'k:', SFtime([accStopIdx_SF_y accStopIdx_SF_y]), [minY maxY], 'k:', 'LineWidth', 1); 389 | plot(SFtime([accStartIdx_SF_z accStartIdx_SF_z]), [minY maxY], 'k:', SFtime([accStopIdx_SF_z accStopIdx_SF_z]), [minY maxY], 'k:', 'LineWidth', 1); 390 | 391 | axis([SFtime(accStartIdx_SF) SFtime(accStopIdx_SF) minY maxY]); 392 | 393 | subplot(4,1,3); 394 | plot(SFtime(accStartIdx_SF:accStopIdx_SF), acc1_y(accStartIdx_SF:accStopIdx_SF), 'c', ... 395 | ILtime(accStartIdx_IL:accStopIdx_IL), acc2_y(accStartIdx_IL:accStopIdx_IL), 'b--'); 396 | set(gca, 'Position', plot43Pos, 'FontSize', 12, 'FontName', 'Times'); 397 | ylabel('acc_y (m/s^2)', 'FontSize', 12, 'FontName', 'Times'); 398 | hold on; 399 | 400 | maxY = max([max(acc1_y(accStartIdx_SF:accStopIdx_SF)), max(acc2_y(accStartIdx_IL:accStopIdx_IL))]); 401 | %maxY = max([1.1*maxY 0.9*maxY]); 402 | minY = min([min(acc1_y(accStartIdx_SF:accStopIdx_SF)), min(acc2_y(accStartIdx_IL:accStopIdx_IL))]); 403 | %minY = min([1.1*minY 0.9*minY]); 404 | plot(SFtime([accStartIdx_SF_x accStartIdx_SF_x]), [minY maxY], 'k:', SFtime([accStopIdx_SF_x accStopIdx_SF_x]), [minY maxY], 'k:', 'LineWidth', 1); 405 | plot(SFtime([accStartIdx_SF_y accStartIdx_SF_y]), [minY maxY], 'k:', SFtime([accStopIdx_SF_y accStopIdx_SF_y]), [minY maxY], 'k:', 'LineWidth', 1); 406 | plot(SFtime([accStartIdx_SF_z accStartIdx_SF_z]), [minY maxY], 'k:', SFtime([accStopIdx_SF_z accStopIdx_SF_z]), [minY maxY], 'k:', 'LineWidth', 1); 407 | axis([SFtime(accStartIdx_SF) SFtime(accStopIdx_SF) minY maxY]); 408 | 409 | subplot(4,1,4); 410 | plot(SFtime(accStartIdx_SF:accStopIdx_SF), acc1_z(accStartIdx_SF:accStopIdx_SF), 'c', ... 411 | ILtime(accStartIdx_IL:accStopIdx_IL), acc2_z(accStartIdx_IL:accStopIdx_IL), 'b--'); 412 | set(gca, 'Position', plot44Pos, 'FontSize', 12, 'FontName', 'Times'); 413 | ylabel('acc_z (m/s^2)', 'FontSize', 12, 'FontName', 'Times'); 414 | xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times'); 415 | hold on; 416 | 417 | maxY = max([max(acc1_z(accStartIdx_SF:accStopIdx_SF)), max(acc2_z(accStartIdx_IL:accStopIdx_IL))]); 418 | %maxY = max([1.1*maxY 0.9*maxY]); 419 | minY = min([min(acc1_z(accStartIdx_SF:accStopIdx_SF)), min(acc2_z(accStartIdx_IL:accStopIdx_IL))]); 420 | %minY = min([1.1*minY 0.9*minY]); 421 | plot(SFtime([accStartIdx_SF_x accStartIdx_SF_x]), [minY maxY], 'k:', SFtime([accStopIdx_SF_x accStopIdx_SF_x]), [minY maxY], 'k:', 'LineWidth', 1); 422 | plot(SFtime([accStartIdx_SF_y accStartIdx_SF_y]), [minY maxY], 'k:', SFtime([accStopIdx_SF_y accStopIdx_SF_y]), [minY maxY], 'k:', 'LineWidth', 1); 423 | plot(SFtime([accStartIdx_SF_z accStartIdx_SF_z]), [minY maxY], 'k:', SFtime([accStopIdx_SF_z accStopIdx_SF_z]), [minY maxY], 'k:', 'LineWidth', 1); 424 | 425 | plot([SFtime(accStartIdx_SF_x)+arrowOffset SFtime(accStopIdx_SF_x)-arrowOffset], [minY minY]+arrowOffset_y, 'k:', ... 426 | SFtime(accStartIdx_SF_x)+arrowOffset, minY+arrowOffset_y, 'k<', SFtime(accStopIdx_SF_x)-arrowOffset, minY+arrowOffset_y, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize); 427 | text(SFtime(accStartIdx_SF_x)+2*arrowOffset, minY-arrowOffset_y, 'x-axis movement', 'FontSize', 12, 'FontName', 'Times'); 428 | 429 | plot([SFtime(accStartIdx_SF_y)+arrowOffset SFtime(accStopIdx_SF_y)-arrowOffset], [minY minY]+arrowOffset_y, 'k:', ... 430 | SFtime(accStartIdx_SF_y)+arrowOffset, minY+arrowOffset_y, 'k<', SFtime(accStopIdx_SF_y)-arrowOffset, minY+arrowOffset_y, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize); 431 | text(SFtime(accStartIdx_SF_y)+2*arrowOffset, minY-arrowOffset_y, 'y-axis movement', 'FontSize', 12, 'FontName', 'Times'); 432 | 433 | plot([SFtime(accStartIdx_SF_z)+arrowOffset SFtime(accStopIdx_SF_z)-arrowOffset], [minY minY]+arrowOffset_y, 'k:', ... 434 | SFtime(accStartIdx_SF_z)+arrowOffset, minY+arrowOffset_y, 'k<', SFtime(accStopIdx_SF_z)-arrowOffset, minY+arrowOffset_y, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize); 435 | text(SFtime(accStartIdx_SF_z)+2*arrowOffset, minY-arrowOffset_y, 'z-axis movement', 'FontSize', 12, 'FontName', 'Times'); 436 | 437 | axis([SFtime(accStartIdx_SF) SFtime(accStopIdx_SF) minY-4*arrowOffset_y maxY]); 438 | 439 | set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 150]); 440 | set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]); 441 | set(gcf,'PaperPositionMode','auto'); 442 | saveas(gcf, ['figures/test_fig_' int2str(figIdx) '.fig']); 443 | print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps']) 444 | figIdx = figIdx + 1; 445 | 446 | 447 | % DCM states 448 | figure(figIdx); clf; 449 | subplot(3,1,1); 450 | plot(ILtime, x_hist2(:,1), 'b--', SFtime, x_hist1(:,1), 'c', ILtime, -data.InertiaLink.M13, 'm-.', Ktime, R31, 'k--'); 451 | legend('DCM_A', 'DCM_B', 'Inertia-Link', 'Kuka', 'Location', legendLocation([x_hist2(:,1) -data.InertiaLink.M13])); 452 | set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times'); 453 | xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times'); 454 | ylabel('x1 (R31)', 'FontSize', 12, 'FontName', 'Times'); 455 | title('DCM states', 'FontSize', 14, 'FontName', 'Times'); 456 | axis([0 commonDuration -1.1 1.1]); 457 | 458 | subplot(3,1,2); 459 | plot(ILtime, x_hist2(:,2), 'b--', SFtime, x_hist1(:,2), 'c', ILtime, -data.InertiaLink.M23, 'm-.', Ktime, R32, 'k--'); 460 | set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times'); 461 | ylabel('x2 (R32)', 'FontSize', 12, 'FontName', 'Times'); 462 | axis([0 commonDuration -1.1 1.1]); 463 | 464 | subplot(3,1,3); 465 | plot(ILtime, x_hist2(:,3), 'b--', SFtime, x_hist1(:,3), 'c', ILtime, -data.InertiaLink.M33, 'm-.', Ktime, R33, 'k--'); 466 | set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times'); 467 | xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times'); 468 | ylabel('x3 (R33)', 'FontSize', 12, 'FontName', 'Times'); 469 | axis([0 commonDuration -1.1 1.1]); 470 | 471 | set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]); 472 | set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]); 473 | set(gcf,'PaperPositionMode','auto'); 474 | saveas(gcf, ['figures/test_fig_' int2str(figIdx) '.fig']); 475 | print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps']) 476 | figIdx = figIdx + 1; 477 | 478 | 479 | % yaw 480 | disp(' '); 481 | disp('Yaw errors:') 482 | dt = 1/150; 483 | yaw_DCM_error = (ypr_hist2(StartIdx:StopIdx,1)-yaw_in_IL)*180/pi; 484 | yaw_DCM_delta_error = [0; yaw_DCM_error(2:end) - yaw_DCM_error(1:end-1)] ./dt; 485 | yaw_DCM_error_acc = yaw_DCM_error(accStartIdx:accStopIdx)-yaw_DCM_error(accStartIdx); 486 | yaw_DCM_error_rot = yaw_DCM_error(rotStartIdx:rotStopIdx)-yaw_DCM_error(rotStartIdx); 487 | yaw_DCM_RMSE_acc = sqrt(mean(yaw_DCM_error_acc.^2)) 488 | yaw_DCM_RMSE_rot = sqrt(mean(yaw_DCM_error_rot.^2)) 489 | 490 | yaw_sf_error = yaw_sf_in_IL - yaw_in_IL*180/pi; 491 | yaw_sf_delta_error = [0; yaw_sf_error(2:end) - yaw_sf_error(1:end-1)] ./dt; 492 | yaw_sf_error_acc = yaw_sf_error(accStartIdx:accStopIdx)-yaw_sf_error(accStartIdx); 493 | yaw_sf_error_rot = yaw_sf_error(rotStartIdx:rotStopIdx)-yaw_sf_error(rotStartIdx); 494 | yaw_sf_RMSE_acc = sqrt(mean(yaw_sf_error_acc.^2)) 495 | yaw_sf_RMSE_rot = sqrt(mean(yaw_sf_error_rot.^2)) 496 | 497 | yaw_mad_B_error = euler_mad_B(StartIdx:StopIdx,3) - yaw_in_IL*180/pi; 498 | yaw_mad_B_delta_error = [0; yaw_mad_B_error(2:end) - yaw_mad_B_error(1:end-1)] ./dt; 499 | yaw_mad_B_error_acc = yaw_mad_B_error(accStartIdx:accStopIdx)-yaw_mad_B_error(accStartIdx); 500 | yaw_mad_B_error_rot = yaw_mad_B_error(rotStartIdx:rotStopIdx)-yaw_mad_B_error(rotStartIdx); 501 | yaw_mad_B_RMSE_acc = sqrt(mean(yaw_mad_B_error_acc.^2)) 502 | yaw_mad_B_RMSE_rot = sqrt(mean(yaw_mad_B_error_rot.^2)) 503 | 504 | yaw_mah_B_error = euler_mah_B(StartIdx:StopIdx,3) - yaw_in_IL*180/pi; 505 | yaw_mah_B_delta_error = [0; yaw_mah_B_error(2:end) - yaw_mah_B_error(1:end-1)] ./dt; 506 | yaw_mah_B_error_acc = yaw_mah_B_error(accStartIdx:accStopIdx)-yaw_mah_B_error(accStartIdx); 507 | yaw_mah_B_error_rot = yaw_mah_B_error(rotStartIdx:rotStopIdx)-yaw_mah_B_error(rotStartIdx); 508 | yaw_mah_B_RMSE_acc = sqrt(mean(yaw_mah_B_error_acc.^2)) 509 | yaw_mah_B_RMSE_rot = sqrt(mean(yaw_mah_B_error_rot.^2)) 510 | 511 | yaw_mad_A_error = euler_mad_A(StartIdx:StopIdx,3) - yaw_in_IL*180/pi; 512 | yaw_mad_A_delta_error = [0; yaw_mad_A_error(2:end) - yaw_mad_A_error(1:end-1)] ./dt; 513 | yaw_mad_A_error_acc = yaw_mad_A_error(accStartIdx:accStopIdx)-yaw_mad_A_error(accStartIdx); 514 | yaw_mad_A_error_rot = yaw_mad_A_error(rotStartIdx:rotStopIdx)-yaw_mad_A_error(rotStartIdx); 515 | yaw_mad_A_RMSE_acc = sqrt(mean(yaw_mad_A_error_acc.^2)) 516 | yaw_mad_A_RMSE_rot = sqrt(mean(yaw_mad_A_error_rot.^2)) 517 | 518 | yaw_mah_A_error = euler_mah_A(StartIdx:StopIdx,3) - yaw_in_IL*180/pi; 519 | yaw_mah_A_delta_error = [0; yaw_mah_A_error(2:end) - yaw_mah_A_error(1:end-1)] ./dt; 520 | yaw_mah_A_error_acc = yaw_mah_A_error(accStartIdx:accStopIdx)-yaw_mah_A_error(accStartIdx); 521 | yaw_mah_A_error_rot = yaw_mah_A_error(rotStartIdx:rotStopIdx)-yaw_mah_A_error(rotStartIdx); 522 | yaw_mah_A_RMSE_acc = sqrt(mean(yaw_mah_A_error_acc.^2)) 523 | yaw_mah_A_RMSE_rot = sqrt(mean(yaw_mah_A_error_rot.^2)) 524 | 525 | yaw_IL_error = (yaw_IL(StartIdx:StopIdx)-yaw_in_IL)*180/pi; 526 | yaw_IL_delta_error = [0; yaw_IL_error(2:end) - yaw_IL_error(1:end-1)] ./dt; 527 | yaw_IL_error_acc = yaw_IL_error(accStartIdx:accStopIdx)-yaw_IL_error(accStartIdx); 528 | yaw_IL_error_rot = yaw_IL_error(rotStartIdx:rotStopIdx)-yaw_IL_error(rotStartIdx); 529 | yaw_IL_RMSE_acc = sqrt(mean(yaw_IL_error_acc.^2)) 530 | yaw_IL_RMSE_rot = sqrt(mean(yaw_IL_error_rot.^2)) 531 | 532 | %pitch 533 | disp(' '); 534 | disp('Pitch errors:') 535 | pitch_DCM_error = (ypr_hist2(StartIdx:StopIdx,2)-pitch_in_IL)*180/pi; 536 | pitch_DCM_RMSE_acc = sqrt(mean(pitch_DCM_error(accStartIdx:accStopIdx).^2)) 537 | pitch_DCM_RMSE_rot = sqrt(mean(pitch_DCM_error(rotStartIdx:rotStopIdx).^2)) 538 | 539 | pitch_sf_error = pitch_sf_in_IL - pitch_in_IL*180/pi; 540 | pitch_sf_RMSE_acc = sqrt(mean(pitch_sf_error(accStartIdx:accStopIdx).^2)) 541 | pitch_sf_RMSE_rot = sqrt(mean(pitch_sf_error(rotStartIdx:rotStopIdx).^2)) 542 | 543 | pitch_mad_B_error = euler_mad_B(StartIdx:StopIdx,2) - pitch_in_IL*180/pi; 544 | pitch_mad_B_RMSE_acc = sqrt(mean(pitch_mad_B_error(accStartIdx:accStopIdx).^2)) 545 | pitch_mad_B_RMSE_rot = sqrt(mean(pitch_mad_B_error(rotStartIdx:rotStopIdx).^2)) 546 | 547 | pitch_mah_B_error = euler_mah_B(StartIdx:StopIdx,2) - pitch_in_IL*180/pi; 548 | pitch_mah_B_RMSE_acc = sqrt(mean(pitch_mah_B_error(accStartIdx:accStopIdx).^2)) 549 | pitch_mah_B_RMSE_rot = sqrt(mean(pitch_mah_B_error(rotStartIdx:rotStopIdx).^2)) 550 | 551 | pitch_mad_A_error = euler_mad_A(StartIdx:StopIdx,2) - pitch_in_IL*180/pi; 552 | pitch_mad_A_RMSE_acc = sqrt(mean(pitch_mad_A_error(accStartIdx:accStopIdx).^2)) 553 | pitch_mad_A_RMSE_rot = sqrt(mean(pitch_mad_A_error(rotStartIdx:rotStopIdx).^2)) 554 | 555 | pitch_mah_A_error = euler_mah_A(StartIdx:StopIdx,2) - pitch_in_IL*180/pi; 556 | pitch_mah_A_RMSE_acc = sqrt(mean(pitch_mah_A_error(accStartIdx:accStopIdx).^2)) 557 | pitch_mah_A_RMSE_rot = sqrt(mean(pitch_mah_A_error(rotStartIdx:rotStopIdx).^2)) 558 | 559 | pitch_IL_error = (pitch_IL(StartIdx:StopIdx)-pitch_in_IL)*180/pi; 560 | pitch_IL_RMSE_acc = sqrt(mean(pitch_IL_error(accStartIdx:accStopIdx).^2)) 561 | pitch_IL_RMSE_rot = sqrt(mean(pitch_IL_error(rotStartIdx:rotStopIdx).^2)) 562 | 563 | %roll 564 | disp(' '); 565 | disp('Roll errors:') 566 | roll_DCM_error = (ypr_hist2(StartIdx:StopIdx,3)-roll_in_IL)*180/pi; 567 | roll_DCM_RMSE_acc = sqrt(mean(roll_DCM_error(accStartIdx:accStopIdx).^2)) 568 | roll_DCM_RMSE_rot = sqrt(mean(roll_DCM_error(rotStartIdx:rotStopIdx).^2)) 569 | 570 | roll_sf_error = roll_sf_in_IL - roll_in_IL*180/pi; 571 | roll_sf_RMSE_acc = sqrt(mean(roll_sf_error(accStartIdx:accStopIdx).^2)) 572 | roll_sf_RMSE_rot = sqrt(mean(roll_sf_error(rotStartIdx:rotStopIdx).^2)) 573 | 574 | roll_mad_B_error = euler_mad_B(StartIdx:StopIdx,1) - roll_in_IL*180/pi; 575 | roll_mad_B_RMSE_acc = sqrt(mean(roll_mad_B_error(accStartIdx:accStopIdx).^2)) 576 | roll_mad_B_RMSE_rot = sqrt(mean(roll_mad_B_error(rotStartIdx:rotStopIdx).^2)) 577 | 578 | roll_mah_B_error = euler_mah_B(StartIdx:StopIdx,1) - roll_in_IL*180/pi; 579 | roll_mah_B_RMSE_acc = sqrt(mean(roll_mah_B_error(accStartIdx:accStopIdx).^2)) 580 | roll_mah_B_RMSE_rot = sqrt(mean(roll_mah_B_error(rotStartIdx:rotStopIdx).^2)) 581 | 582 | roll_mad_A_error = euler_mad_A(StartIdx:StopIdx,1) - roll_in_IL*180/pi; 583 | roll_mad_A_RMSE_acc = sqrt(mean(roll_mad_A_error(accStartIdx:accStopIdx).^2)) 584 | roll_mad_A_RMSE_rot = sqrt(mean(roll_mad_A_error(rotStartIdx:rotStopIdx).^2)) 585 | 586 | roll_mah_A_error = euler_mah_A(StartIdx:StopIdx,1) - roll_in_IL*180/pi; 587 | roll_mah_A_RMSE_acc = sqrt(mean(roll_mah_A_error(accStartIdx:accStopIdx).^2)) 588 | roll_mah_A_RMSE_rot = sqrt(mean(roll_mah_A_error(rotStartIdx:rotStopIdx).^2)) 589 | 590 | roll_IL_error = (roll_IL(StartIdx:StopIdx)-roll_in_IL)*180/pi; 591 | roll_IL_RMSE_acc = sqrt(mean(roll_IL_error(accStartIdx:accStopIdx).^2)) 592 | roll_IL_RMSE_rot = sqrt(mean(roll_IL_error(rotStartIdx:rotStopIdx).^2)) 593 | 594 | disp(' '); 595 | disp('Error matrices:') 596 | RMSE_acc = [yaw_DCM_RMSE_acc yaw_sf_RMSE_acc yaw_mad_A_RMSE_acc yaw_mad_B_RMSE_acc yaw_mah_A_RMSE_acc yaw_mah_B_RMSE_acc yaw_IL_RMSE_acc; ... 597 | pitch_DCM_RMSE_acc pitch_sf_RMSE_acc pitch_mad_A_RMSE_acc pitch_mad_B_RMSE_acc pitch_mah_A_RMSE_acc pitch_mah_B_RMSE_acc pitch_IL_RMSE_acc; ... 598 | roll_DCM_RMSE_acc roll_sf_RMSE_acc roll_mad_A_RMSE_acc roll_mad_B_RMSE_acc roll_mah_A_RMSE_acc roll_mah_B_RMSE_acc roll_IL_RMSE_acc]; 599 | 600 | disp('RMSEs of the acceleration test'); 601 | textCell = [{'DCM_A', 'DCM_B', 'Madgwick_A', 'Madgwick_B', 'Mahony_A', 'Mahony_B', 'Inertia-Link'}; cell(size(RMSE_acc))]; 602 | for i = 1:size(RMSE_acc,1) 603 | for j = 1:size(RMSE_acc,2) 604 | textCell{i+1,j} = num2str(RMSE_acc(i,j), '%.2f'); 605 | end 606 | end 607 | disp(textCell'); 608 | 609 | RMSE_rot = [yaw_DCM_RMSE_rot yaw_sf_RMSE_rot yaw_mad_A_RMSE_rot yaw_mad_B_RMSE_rot yaw_mah_A_RMSE_rot yaw_mah_B_RMSE_rot yaw_IL_RMSE_rot; ... 610 | pitch_DCM_RMSE_rot pitch_sf_RMSE_rot pitch_mad_A_RMSE_rot pitch_mad_B_RMSE_rot pitch_mah_A_RMSE_rot pitch_mah_B_RMSE_rot pitch_IL_RMSE_rot; ... 611 | roll_DCM_RMSE_rot roll_sf_RMSE_rot roll_mad_A_RMSE_rot roll_mad_B_RMSE_rot roll_mah_A_RMSE_rot roll_mah_B_RMSE_rot roll_IL_RMSE_rot]; 612 | 613 | disp('RMSEs of the rotation test'); 614 | textCell = [{'DCM_A', 'DCM_B', 'Madgwick_A', 'Madgwick_B', 'Mahony_A', 'Mahony_B', 'Inertia-Link'}; cell(size(RMSE_rot))]; 615 | for i = 1:size(RMSE_rot,1) 616 | for j = 1:size(RMSE_rot,2) 617 | textCell{i+1,j} = num2str(RMSE_rot(i,j), '%.2f'); 618 | end 619 | end 620 | disp(textCell'); 621 | 622 | 623 | % box and whiskers plot for the acceleration test 624 | groups_yaw = {'DCM_A', 'Madgwick_A', 'Mahony_A', 'DCM_B','Madgwick_B', 'Mahony_B', 'Inertia-Link_ '}; 625 | 626 | groups_rollpitch = cell(1, 2*size(groups_yaw,2)); 627 | groups_rollpitch(1:2:end) = groups_yaw; 628 | groups_rollpitch(2:2:end) = groups_yaw; 629 | 630 | figure(figIdx); clf; 631 | subplot(2,1,1); 632 | boxplot([yaw_DCM_error_acc, yaw_mad_A_error_acc, yaw_mah_A_error_acc, yaw_sf_error_acc, yaw_mad_B_error_acc, yaw_mah_B_error_acc, yaw_IL_error_acc], ... 633 | groups_yaw, 'whisker', 15); 634 | txt = findobj(gca,'Type','text'); 635 | set(txt, 'FontSize', 12, 'FontName', 'Times', 'Interpreter', 'tex', 'VerticalAlignment', 'middle'); 636 | set(gca, 'Position', plot21Pos, 'FontSize', 12, 'FontName', 'Times'); 637 | title('Error statistics in the acceleration test', 'FontSize', 14, 'FontName', 'Times'); 638 | ylabel('yaw errors (deg)'); 639 | 640 | subplot(2,1,2); 641 | 642 | boxplot([pitch_DCM_error(accStartIdx:accStopIdx), roll_DCM_error(accStartIdx:accStopIdx), ... 643 | pitch_mad_A_error(accStartIdx:accStopIdx), roll_mad_A_error(accStartIdx:accStopIdx), ... 644 | pitch_mah_A_error(accStartIdx:accStopIdx), roll_mah_A_error(accStartIdx:accStopIdx), ... 645 | pitch_sf_error(accStartIdx:accStopIdx), roll_sf_error(accStartIdx:accStopIdx), ... 646 | pitch_mad_B_error(accStartIdx:accStopIdx), roll_mad_B_error(accStartIdx:accStopIdx), ... 647 | pitch_mah_B_error(accStartIdx:accStopIdx), roll_mah_B_error(accStartIdx:accStopIdx), ... 648 | pitch_IL_error(accStartIdx:accStopIdx), roll_IL_error(accStartIdx:accStopIdx)], ... 649 | groups_rollpitch, 'whisker', 15); 650 | 651 | txt = findobj(gca,'Type','text'); 652 | set(txt, 'FontSize', 12, 'FontName', 'Times', 'Interpreter', 'tex', 'VerticalAlignment', 'middle'); 653 | 654 | set(gca, 'Position', plot22Pos, 'FontSize', 12, 'FontName', 'Times'); 655 | ylabel('combined roll and pitch errors (deg)'); 656 | 657 | set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]); 658 | set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]); 659 | set(gcf,'PaperPositionMode','auto'); 660 | saveas(gcf, ['figures/test_fig_' int2str(figIdx) '.fig']); 661 | print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps']) 662 | figIdx = figIdx + 1; 663 | 664 | 665 | % box and whiskers plot for the rotation test 666 | figure(figIdx); clf; 667 | subplot(2,1,1); 668 | boxplot([yaw_DCM_error_rot, yaw_mad_A_error_rot, yaw_mah_A_error_rot, yaw_sf_error_rot, yaw_mad_B_error_rot, yaw_mah_B_error_rot, yaw_IL_error_rot], ... 669 | groups_yaw, 'whisker', 15); 670 | txt = findobj(gca,'Type','text'); 671 | set(txt, 'FontSize', 12, 'FontName', 'Times', 'Interpreter', 'tex', 'VerticalAlignment', 'middle'); 672 | 673 | set(gca, 'Position', plot21Pos, 'FontSize', 12, 'FontName', 'Times'); 674 | title('Error statistics in the rotation test', 'FontSize', 14, 'FontName', 'Times'); 675 | ylabel('yaw errors (deg)'); 676 | 677 | subplot(2,1,2); 678 | boxplot([pitch_DCM_error(rotStartIdx:rotStopIdx), roll_DCM_error(rotStartIdx:rotStopIdx), ... 679 | pitch_mad_A_error(rotStartIdx:rotStopIdx), roll_mad_A_error(rotStartIdx:rotStopIdx), ... 680 | pitch_mah_A_error(rotStartIdx:rotStopIdx), roll_mah_A_error(rotStartIdx:rotStopIdx), ... 681 | pitch_sf_error(rotStartIdx:rotStopIdx), roll_sf_error(rotStartIdx:rotStopIdx), ... 682 | pitch_mad_B_error(rotStartIdx:rotStopIdx), roll_mad_B_error(rotStartIdx:rotStopIdx), ... 683 | pitch_mah_B_error(rotStartIdx:rotStopIdx), roll_mah_B_error(rotStartIdx:rotStopIdx), ... 684 | pitch_IL_error(rotStartIdx:rotStopIdx), roll_IL_error(rotStartIdx:rotStopIdx)], ... 685 | groups_rollpitch, 'whisker', 15); 686 | txt = findobj(gca,'Type','text'); 687 | set(txt, 'FontSize', 12, 'FontName', 'Times', 'Interpreter', 'tex', 'VerticalAlignment', 'middle'); 688 | 689 | set(gca, 'Position', plot22Pos, 'FontSize', 12, 'FontName', 'Times'); 690 | ylabel('combined roll and pitch errors (deg)'); 691 | 692 | set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]); 693 | set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]); 694 | set(gcf,'PaperPositionMode','auto'); 695 | saveas(gcf, ['figures/test_fig_' int2str(figIdx) '.fig']); 696 | print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps']) 697 | figIdx = figIdx + 1; 698 | 699 | 700 | % correlation tests for delays 701 | yaw_ref = yaw_in_IL*180/pi; 702 | pitch_ref = pitch_in_IL*180/pi; 703 | roll_ref = roll_in_IL*180/pi; 704 | 705 | yaw_dcm = ypr_hist2(StartIdx:StopIdx,1)*180/pi; 706 | pitch_dcm = ypr_hist2(StartIdx:StopIdx,2)*180/pi; 707 | roll_dcm = ypr_hist2(StartIdx:StopIdx,3)*180/pi; 708 | 709 | madgwick = euler_mad_A(StartIdx:StopIdx,:); 710 | mahony = euler_mah_A(StartIdx:StopIdx,:); 711 | 712 | yaw_il = yaw_IL(StartIdx:StopIdx)*180/pi; 713 | pitch_il = pitch_IL(StartIdx:StopIdx)*180/pi; 714 | roll_il = roll_IL(StartIdx:StopIdx)*180/pi; 715 | 716 | n_lags = 30; 717 | [c_dcm_yaw,lags] = rootMeanSquaredErrors(yaw_dcm, yaw_ref, n_lags); 718 | [min_c_dcm_yaw, min_c_dcm_yaw_idx] = min(c_dcm_yaw); 719 | c_madgwick_yaw = rootMeanSquaredErrors(madgwick(:,3), yaw_ref, n_lags); 720 | [min_c_madgwick_yaw, min_c_madgwick_yaw_idx] = min(c_madgwick_yaw); 721 | c_mahony_yaw = rootMeanSquaredErrors(mahony(:,3), yaw_ref, n_lags); 722 | [min_c_mahony_yaw, min_c_mahony_yaw_idx] = min(c_mahony_yaw); 723 | c_il_yaw = rootMeanSquaredErrors(yaw_il, yaw_ref, n_lags); 724 | [min_c_il_yaw, min_c_il_yaw_idx] = min(c_il_yaw); 725 | 726 | c_dcm_pitch = rootMeanSquaredErrors(pitch_dcm, pitch_ref, n_lags); 727 | [min_c_dcm_pitch, min_c_dcm_pitch_idx] = min(c_dcm_pitch); 728 | c_madgwick_pitch = rootMeanSquaredErrors(madgwick(:,2), pitch_ref, n_lags); 729 | [min_c_madgwick_pitch, min_c_madgwick_pitch_idx] = min(c_madgwick_pitch); 730 | c_mahony_pitch = rootMeanSquaredErrors(mahony(:,2), pitch_ref, n_lags); 731 | [min_c_mahony_pitch, min_c_mahony_pitch_idx] = min(c_mahony_pitch); 732 | c_il_pitch = rootMeanSquaredErrors(pitch_il, pitch_ref, n_lags); 733 | [min_c_il_pitch, min_c_il_pitch_idx] = min(c_il_pitch); 734 | 735 | c_dcm_roll = rootMeanSquaredErrors(roll_dcm, roll_ref, n_lags); 736 | [min_c_dcm_roll, min_c_dcm_roll_idx] = min(c_dcm_roll); 737 | c_madgwick_roll = rootMeanSquaredErrors(madgwick(:,1), roll_ref, n_lags); 738 | [min_c_madgwick_roll, min_c_madgwick_roll_idx] = min(c_madgwick_roll); 739 | c_mahony_roll = rootMeanSquaredErrors(mahony(:,1), roll_ref, n_lags); 740 | [min_c_mahony_roll, min_c_mahony_roll_idx] = min(c_mahony_roll); 741 | c_il_roll = rootMeanSquaredErrors(roll_il, roll_ref, n_lags); 742 | [min_c_il_roll, min_c_il_roll_idx] = min(c_il_roll); 743 | 744 | lags = lags*dt; 745 | 746 | 747 | % mean squared errors plot 748 | figure(figIdx); clf; 749 | subplot(3,1,1); 750 | plot(lags, c_dcm_yaw, 'b', lags, c_madgwick_yaw, 'g--', lags, c_mahony_yaw, 'r:', lags, c_il_yaw, 'm-.', 'LineWidth', 1, 'MarkerSize', markerSize); 751 | set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times'); 752 | title('RMSEs to the reference measurement as a function of time delay', 'FontSize', 14, 'FontName', 'Times'); 753 | ylabel('yaw RMSE (deg)'); 754 | legend('DCM', 'Madgwick', 'Mahony', 'Inertia-Link', 'Location', legendLocation([c_dcm_yaw' c_madgwick_yaw' c_mahony_yaw' c_il_yaw'])); 755 | hold on; 756 | plot(lags(min_c_dcm_yaw_idx), c_dcm_yaw(min_c_dcm_yaw_idx), 'b*', ... 757 | lags(min_c_madgwick_yaw_idx), c_madgwick_yaw(min_c_madgwick_yaw_idx), 'g^', ... 758 | lags(min_c_mahony_yaw_idx), c_mahony_yaw(min_c_mahony_yaw_idx), 'ro', ... 759 | lags(min_c_il_yaw_idx), c_il_yaw(min_c_il_yaw_idx), 'mp', 'LineWidth', 1, 'MarkerSize', markerSize); 760 | 761 | 762 | subplot(3,1,2); 763 | plot(lags, c_dcm_pitch, 'b', lags, c_madgwick_pitch, 'g--', lags, c_mahony_pitch, 'r:', lags, c_il_pitch, 'm-.', 'LineWidth', 1, 'MarkerSize', markerSize); 764 | set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times'); 765 | ylabel('pitch RMSE (deg)'); 766 | hold on; 767 | plot(lags(min_c_dcm_pitch_idx), c_dcm_pitch(min_c_dcm_pitch_idx), 'b*', ... 768 | lags(min_c_madgwick_pitch_idx), c_madgwick_pitch(min_c_madgwick_pitch_idx), 'g^', ... 769 | lags(min_c_mahony_pitch_idx), c_mahony_pitch(min_c_mahony_pitch_idx), 'ro', ... 770 | lags(min_c_il_pitch_idx), c_il_pitch(min_c_il_pitch_idx), 'mp', 'LineWidth', 1, 'MarkerSize', markerSize); 771 | 772 | 773 | subplot(3,1,3); 774 | plot(lags, c_dcm_roll, 'b', lags, c_madgwick_roll, 'g--', lags, c_mahony_roll, 'r:', lags, c_il_roll, 'm-.', 'LineWidth', 1, 'MarkerSize', markerSize); 775 | set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times'); 776 | ylabel('roll RMSE (deg)'); 777 | xlabel('time delay to reference (s)'); 778 | hold on; 779 | plot(lags(min_c_dcm_roll_idx), c_dcm_roll(min_c_dcm_roll_idx), 'b*', ... 780 | lags(min_c_madgwick_roll_idx), c_madgwick_roll(min_c_madgwick_roll_idx), 'g^', ... 781 | lags(min_c_mahony_roll_idx), c_mahony_roll(min_c_mahony_roll_idx), 'ro', ... 782 | lags(min_c_il_roll_idx), c_il_roll(min_c_il_roll_idx), 'mp', 'LineWidth', 1, 'MarkerSize', markerSize); 783 | 784 | 785 | set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]); 786 | set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]); 787 | set(gcf,'PaperPositionMode','auto'); 788 | saveas(gcf, ['figures/test_fig_' int2str(figIdx) '.fig']); 789 | print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps']) 790 | figIdx = figIdx + 1; 791 | 792 | 793 | % Yaw, pitch and roll plot 794 | figure(figIdx); clf; 795 | subplot(3,1,1); 796 | plot(time, yaw_dcm, 'b', time, madgwick(:,3), 'g--', time, mahony(:,3), 'r:', time, yaw_il, 'm-.', time, yaw_ref, 'k--', 'LineWidth', 1); 797 | set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times'); 798 | legend('DCM', 'Madgwick', 'Mahony', 'Inertia-Link', 'Reference', 'Location', legendLocation([yaw_dcm yaw_ref madgwick(:,3) mahony(:,3)])); 799 | hold on; 800 | maxY = max([max(yaw_dcm), max(madgwick(:,3)), max(mahony(:,3)), max(yaw_il), max(yaw_ref)]); 801 | maxY = max([1.1*maxY 0.9*maxY]); 802 | minY = min([min(yaw_dcm), min(madgwick(:,3)), min(mahony(:,3)), min(yaw_il), min(yaw_ref)]); 803 | minY = min([1.1*minY 0.9*minY]); 804 | plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1); 805 | plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1); 806 | ylabel('yaw (deg)', 'FontSize', 12, 'FontName', 'Times'); 807 | title('Euler angles', 'FontSize', 14, 'FontName', 'Times'); 808 | axis([time(1) time(end) minY maxY]); 809 | 810 | subplot(3,1,2); 811 | plot(time, pitch_dcm, 'b', time, madgwick(:,2), 'g--', time, mahony(:,2), 'r:', time, pitch_il, 'm-.', time, pitch_ref, 'k--', 'LineWidth', 1); 812 | hold on; 813 | maxY = max([max(pitch_dcm), max(madgwick(:,2)), max(mahony(:,2)), max(pitch_il), max(pitch_ref)]); 814 | maxY = max([1.1*maxY 0.9*maxY]); 815 | minY = min([min(pitch_dcm), min(madgwick(:,2)), min(mahony(:,2)), min(pitch_il), min(pitch_ref)]); 816 | minY = min([1.1*minY 0.9*minY]); 817 | plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1); 818 | plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1); 819 | set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times'); 820 | ylabel('pitch (deg)', 'FontSize', 12, 'FontName', 'Times'); 821 | axis([time(1) time(end) minY maxY]); 822 | 823 | subplot(3,1,3); 824 | plot(time, roll_dcm, 'b', time, madgwick(:,1), 'g--', time, mahony(:,1), 'r:', time, roll_il, 'm-.', time, roll_ref, 'k--', 'LineWidth', 1); 825 | set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times'); 826 | hold on; 827 | maxY = max([max(roll_dcm), max(madgwick(:,1)), max(mahony(:,1)), max(roll_il), max(roll_ref)]); 828 | maxY = max([1.1*maxY 0.9*maxY]); 829 | minY = min([min(roll_dcm), min(madgwick(:,1)), min(mahony(:,1)), min(roll_il), min(roll_ref)]); 830 | minY = min([1.1*minY 0.9*minY]); 831 | arrowOffset = 5; 832 | plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1); 833 | plot([time(accStartIdx)+arrowOffset time(accStopIdx)-arrowOffset], [minY minY]+arrowOffset, 'k:', ... 834 | time(accStartIdx)+arrowOffset, minY+arrowOffset, 'k<', time(accStopIdx)-arrowOffset, minY+arrowOffset, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize); 835 | text(time(accStartIdx)+2*arrowOffset, minY-arrowOffset, 'Acceleration test', 'FontSize', 12, 'FontName', 'Times'); 836 | plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1); 837 | plot([time(rotStartIdx)+arrowOffset time(rotStopIdx)-arrowOffset], [minY minY]+arrowOffset, 'k:', ... 838 | time(rotStartIdx)+arrowOffset, minY+arrowOffset, 'k<', time(rotStopIdx)-arrowOffset, minY+arrowOffset, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize); 839 | text(time(rotStartIdx)+2*arrowOffset, minY-arrowOffset, 'Rotation test', 'FontSize', 12, 'FontName', 'Times'); 840 | xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times'); 841 | ylabel('roll (deg)', 'FontSize', 12, 'FontName', 'Times'); 842 | 843 | axis([time(1) time(end) minY-20 maxY]); 844 | 845 | set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]); 846 | set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]); 847 | set(gcf,'PaperPositionMode','auto'); 848 | saveas(gcf, ['figures/test_fig_' int2str(figIdx) '.fig']); 849 | print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps']) 850 | figIdx = figIdx + 1; 851 | 852 | 853 | % error plot with reference 854 | figure(figIdx); clf; 855 | subplot(4,1,1); 856 | plot(time, yaw_ref, 'b', time, pitch_ref, 'g--', time, roll_ref, 'r:', 'LineWidth', 1); 857 | set(gca, 'Position', plot41Pos, 'FontSize', 12, 'FontName', 'Times'); 858 | legend('yaw', 'pitch', 'roll', 'Location', legendLocation([yaw_ref pitch_ref roll_ref])); 859 | title('The reference measurement and measurement errors', 'FontSize', 14, 'FontName', 'Times'); 860 | ylabel('reference angles (deg)', 'FontSize', 12, 'FontName', 'Times'); 861 | 862 | hold on; 863 | maxY = max([max(yaw_ref), max(pitch_ref), max(roll_ref)]); 864 | maxY = max([1.1*maxY 0.9*maxY]); 865 | minY = min([min(yaw_ref), min(pitch_ref), min(roll_ref)]); 866 | minY = min([1.1*minY 0.9*minY]); 867 | arrowOffset = 5; 868 | plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1); 869 | plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1); 870 | 871 | axis([time(1) time(end) minY maxY]); 872 | 873 | 874 | subplot(4,1,2); 875 | plot(time, yaw_DCM_error, 'b', time, yaw_mad_A_error, 'g--', time, yaw_mah_A_error, 'r:', time, yaw_IL_error, 'm-.', 'LineWidth', 1, 'EraseMode', 'xor'); 876 | set(gca, 'Position', plot42Pos, 'FontSize', 12, 'FontName', 'Times'); 877 | legend('DCM', 'Madgwick', 'Mahony', 'Inertia-Link', 'Location', legendLocation([yaw_DCM_error yaw_IL_error yaw_mad_A_error yaw_mah_A_error])); 878 | hold on; 879 | ylabel('yaw errors (deg)', 'FontSize', 12, 'FontName', 'Times'); 880 | set(gca, 'XLim', [time(1) time(end)]); 881 | 882 | maxY = max([max(yaw_DCM_error), max(yaw_mad_A_error), max(yaw_mah_A_error), max(yaw_IL_error)]); 883 | maxY = max([1.1*maxY 0.9*maxY]); 884 | minY = min([min(yaw_DCM_error), min(yaw_mad_A_error), min(yaw_mah_A_error), min(yaw_IL_error)]); 885 | minY = min([1.1*minY 0.9*minY]); 886 | 887 | plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1); 888 | plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1); 889 | 890 | axis([time(1) time(end) minY maxY]); 891 | 892 | 893 | subplot(4,1,3); 894 | plot(time, pitch_DCM_error, 'b', time, pitch_mad_A_error, 'g--', time, pitch_mah_A_error, 'r:', time, pitch_IL_error, 'm-.', 'LineWidth', 1, 'EraseMode', 'xor'); 895 | hold on; 896 | set(gca, 'Position', plot43Pos, 'FontSize', 12, 'FontName', 'Times'); 897 | ylabel('pitch errors (deg)', 'FontSize', 12, 'FontName', 'Times'); 898 | 899 | maxY = max([max(pitch_DCM_error), max(pitch_mad_A_error), max(pitch_mah_A_error), max(pitch_IL_error)]); 900 | maxY = max([1.1*maxY 0.9*maxY]); 901 | minY = min([min(pitch_DCM_error), min(pitch_mad_A_error), min(pitch_mah_A_error), min(pitch_IL_error)]); 902 | minY = min([1.1*minY 0.9*minY]); 903 | 904 | plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1); 905 | plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1); 906 | 907 | axis([time(1) time(end) minY maxY]); 908 | 909 | 910 | subplot(4,1,4); 911 | plot(time, roll_DCM_error, 'b', time, roll_mad_A_error, 'g--', time, roll_mah_A_error, 'r:', time, roll_IL_error, 'm-.', 'LineWidth', 1, 'EraseMode', 'xor'); 912 | hold on; 913 | set(gca, 'Position', plot44Pos, 'FontSize', 12, 'FontName', 'Times'); 914 | xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times'); 915 | ylabel('roll errors (deg)', 'FontSize', 12, 'FontName', 'Times'); 916 | 917 | maxY = max([max(roll_DCM_error), max(roll_mad_A_error), max(roll_mah_A_error), max(roll_IL_error)]); 918 | maxY = max([1.1*maxY 0.9*maxY]); 919 | minY = min([min(roll_DCM_error), min(roll_mad_A_error), min(roll_mah_A_error), min(roll_IL_error)]); 920 | minY = min([1.1*minY 0.9*minY]); 921 | 922 | arrowOffset_y = 0.5; 923 | 924 | plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1); 925 | plot([time(accStartIdx)+arrowOffset time(accStopIdx)-arrowOffset], [minY minY]+arrowOffset_y, 'k:', ... 926 | time(accStartIdx)+arrowOffset, minY+arrowOffset_y, 'k<', time(accStopIdx)-arrowOffset, minY+arrowOffset_y, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize); 927 | text(time(accStartIdx)+2*arrowOffset, minY-arrowOffset_y, 'Acceleration test', 'FontSize', 12, 'FontName', 'Times'); 928 | plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1); 929 | plot([time(rotStartIdx)+arrowOffset time(rotStopIdx)-arrowOffset], [minY minY]+arrowOffset_y, 'k:', ... 930 | time(rotStartIdx)+arrowOffset, minY+arrowOffset_y, 'k<', time(rotStopIdx)-arrowOffset, minY+arrowOffset_y, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize); 931 | text(time(rotStartIdx)+2*arrowOffset, minY-arrowOffset_y, 'Rotation test', 'FontSize', 12, 'FontName', 'Times'); 932 | 933 | axis([time(1) time(end) minY-4*arrowOffset_y maxY]); 934 | 935 | set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 150]); 936 | set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]); 937 | set(gcf,'PaperPositionMode','auto'); 938 | saveas(gcf, ['figures/test_fig_' int2str(figIdx) '.fig']); 939 | print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps']) 940 | figIdx = figIdx + 1; 941 | 942 | 943 | % bias plot (states with variances) 944 | figure(figIdx); clf; 945 | 946 | x_sigma = sqrt(P_diag_hist2(StartIdx:StopIdx,4)); 947 | y_sigma = sqrt(P_diag_hist2(StartIdx:StopIdx,5)); 948 | z_sigma = sqrt(P_diag_hist2(StartIdx:StopIdx,6)); 949 | 950 | plotLims = 1.25*[mean(x_sigma), mean(y_sigma), mean(z_sigma)]; 951 | 952 | bias_x = x_hist2(StartIdx:StopIdx,4)*180/pi; 953 | bias_y = x_hist2(StartIdx:StopIdx,5)*180/pi; 954 | bias_z = x_hist2(StartIdx:StopIdx,6)*180/pi; 955 | 956 | subplot(3,1,1); 957 | plot([time(1) time(end)], [addedGyroBias addedGyroBias]*180/pi, 'k--', ... 958 | time, bias_x, 'b', time, (addedGyroBias-x_sigma)*180/pi, 'b-.', ... 959 | time, (addedGyroBias+x_sigma)*180/pi, 'b-.', 'LineWidth', 1, 'MarkerSize', markerSize); 960 | h = legend('Reference', 'Estimate', '1-sigma'); 961 | set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times'); 962 | 963 | ylabel('x_b_i_a_s (deg/s)', 'FontSize', 12, 'FontName', 'Times'); 964 | title('Bias estimates and 1-sigma distances of standard deviations', 'FontSize', 14, 'FontName', 'Times'); 965 | set(gca, 'XLim', [time(1) time(end)], 'YLim', [addedGyroBias-plotLims(1) addedGyroBias+plotLims(1)]*180/pi); 966 | 967 | subplot(3,1,2); 968 | plot([time(1) time(end)], [addedGyroBias addedGyroBias]*180/pi, 'k--', ... 969 | time, bias_y, 'b', time, (addedGyroBias-y_sigma)*180/pi, 'b-.', ... 970 | time, (addedGyroBias+y_sigma)*180/pi, 'b-.', 'LineWidth', 1, 'MarkerSize', markerSize); 971 | set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times'); 972 | 973 | ylabel('y_b_i_a_s (deg/s)', 'FontSize', 12, 'FontName', 'Times'); 974 | set(gca, 'XLim', [time(1) time(end)], 'YLim', [addedGyroBias-plotLims(2) addedGyroBias+plotLims(2)]*180/pi); 975 | 976 | subplot(3,1,3); 977 | plot([time(1) time(end)], [addedGyroBias addedGyroBias]*180/pi, 'k--', ... 978 | time, bias_z, 'b', time, (addedGyroBias-z_sigma)*180/pi, 'b-.', ... 979 | time, (addedGyroBias+z_sigma)*180/pi, 'b-.', 'LineWidth', 1, 'MarkerSize', markerSize); 980 | set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times'); 981 | 982 | xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times'); 983 | ylabel('z_b_i_a_s (deg/s)', 'FontSize', 12, 'FontName', 'Times'); 984 | set(gca, 'XLim', [time(1) time(end)], 'YLim', [addedGyroBias-plotLims(3) addedGyroBias+plotLims(3)]*180/pi); 985 | 986 | set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]); 987 | set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]); 988 | set(gcf,'PaperPositionMode','auto'); 989 | saveas(gcf, ['figures/test_fig_' int2str(figIdx) '.fig']); 990 | print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps']) 991 | 992 | 993 | 994 | 995 | -------------------------------------------------------------------------------- /plotIMUsWithKuka_biasTest.m: -------------------------------------------------------------------------------- 1 | %============================================================================ 2 | % Copyright (C) 2015, Heikki Hyyti 3 | % 4 | % Permission is hereby granted, free of charge, to any person obtaining a 5 | % copy of this software and associated documentation files (the "Software"), 6 | % to deal in the Software without restriction, including without limitation 7 | % the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | % and/or sell copies of the Software, and to permit persons to whom the 9 | % Software is furnished to do so, subject to the following conditions: 10 | % 11 | % The above copyright notice and this permission notice shall be included in 12 | % all copies or substantial portions of the Software. 13 | % 14 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | % THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | % FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | % DEALINGS IN THE SOFTWARE. 21 | %============================================================================ 22 | 23 | disp(' '); 24 | disp('If you use the algorithm in any scientific context, please cite:'); 25 | disp('Heikki Hyyti and Arto Visala, "A DCM Based Attitude Estimation Algorithm'); 26 | disp('for Low-Cost MEMS IMUs," International Journal of Navigation and Observation,'); 27 | disp('vol. 2015, Article ID 503814, 18 pages, 2015. http://dx.doi.org/10.1155/2015/503814'); 28 | disp(' '); 29 | 30 | clear all; 31 | close all; 32 | pause(0.1); 33 | 34 | %% Configuration! 35 | % select to test biases using Acceleration test (true) or Rotation test (false) 36 | ACCtest = true; 37 | 38 | % use c/compile.m before setting this to true (tested only with linux) 39 | use_C_versions = false; 40 | 41 | % fetch GPL licensed algortihms in matlab and C from here before use: 42 | % http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ 43 | % Add the C code to c folder and matlab functions in this folder 44 | % (please add also the quaternion library used by Madgwick). 45 | % I apologize this mess. The Madgwick's GPS licenced code is separated from 46 | % this MIT licenced product to avoid infecting this code with GPL licence. 47 | use_comparisonAlgorithms = false; 48 | 49 | 50 | if (ACCtest) 51 | disp('Calculating and plotting IMU bias test for Acceleration data'); 52 | else 53 | disp('Calculating and plotting IMU bias test for Rotation data'); 54 | end 55 | data = load('allData.mat'); 56 | 57 | if (~exist('figures', 'dir')); mkdir('figures'); end; 58 | 59 | ILtime = data.InertiaLink.tv_sec + (data.InertiaLink.tv_msec/1000); 60 | SFtime = data.SparkFun6DOF.tv_sec + (data.SparkFun6DOF.tv_usec / 1000000); 61 | Ktime = data.Kuka.tv_sec + (data.Kuka.tv_usec/1000000); 62 | 63 | firstCommonTime = max([ILtime(1), SFtime(1), Ktime(1)]); 64 | lastCommonTime = min([ILtime(end), SFtime(end), Ktime(end)]); 65 | commonDuration = lastCommonTime - firstCommonTime; 66 | 67 | ILtime = ILtime - firstCommonTime; 68 | SFtime = SFtime - firstCommonTime; 69 | Ktime = Ktime - firstCommonTime; 70 | 71 | 72 | if (ACCtest) 73 | %ACC-test sequence: Linear motion with different accelerations 74 | StartIdx = find(ILtime > 468, 1, 'first'); 75 | StopIdx = find(ILtime > 559, 1, 'first'); 76 | else 77 | %IMU-test sequence: free 6D motion 78 | StartIdx = find(ILtime > 564, 1, 'first'); 79 | StopIdx = find(ILtime > 770, 1, 'first'); 80 | end 81 | 82 | testName = 'Rotation test'; 83 | if (ACCtest) 84 | testName = 'Acceleration test'; 85 | end 86 | 87 | markerSize = 8; 88 | 89 | figPos = [105 105 645 660]; 90 | 91 | plotPos = [0.1 0.15 0.70 0.70]; 92 | 93 | plot21Pos = [0.1 0.53 0.86 0.40]; 94 | plot22Pos = plot21Pos + [0 -0.47 0 0]; 95 | 96 | plot1Pos = [0.1 0.69 0.86 0.25]; 97 | plot2Pos = plot1Pos + [0 -0.31 0 0]; 98 | plot3Pos = plot2Pos + [0 -0.31 0 0]; 99 | 100 | plot41Pos = [0.1 0.75 0.86 0.19]; 101 | plot42Pos = plot41Pos + [0 -0.23 0 0]; 102 | plot43Pos = plot42Pos + [0 -0.23 0 0]; 103 | plot44Pos = plot43Pos + [0 -0.23 0 0]; 104 | 105 | %% getting reference trajectory 106 | R11 = data.Kuka.data_msrCartPos01; 107 | R12 = data.Kuka.data_msrCartPos02; 108 | R13 = data.Kuka.data_msrCartPos03; 109 | x = data.Kuka.data_msrCartPos04; 110 | R21 = data.Kuka.data_msrCartPos05; 111 | R22 = data.Kuka.data_msrCartPos06; 112 | R23 = data.Kuka.data_msrCartPos07; 113 | y = data.Kuka.data_msrCartPos08; 114 | R31 = data.Kuka.data_msrCartPos09; 115 | R32 = data.Kuka.data_msrCartPos10; 116 | R33 = data.Kuka.data_msrCartPos11; 117 | z = data.Kuka.data_msrCartPos12; 118 | 119 | [yaw, pitch, roll] = yawpitchroll(R11, R21, R31, R32, R33); 120 | yaw = un_modulo(yaw, 2*pi); %remove 2pi jumps and make yaw continuous 121 | yaw = yaw - yaw(1); %starting yaw from zero (same as IMU estimates) 122 | 123 | % compute reference angular velocities using the KUKA robot 124 | % (derivate between positions) 125 | gyro = zeros(length(R11),3); 126 | dt = median(Ktime(2:end)-Ktime(1:end-1)); % the most frequently used discretation time 127 | for i = 2:length(R11) 128 | R_last = [R11(i-1), R12(i-1), R13(i-1); ... 129 | R21(i-1), R22(i-1), R23(i-1); ... 130 | R31(i-1), R32(i-1), R33(i-1)]; 131 | R_cur = [R11(i), R12(i), R13(i); ... 132 | R21(i), R22(i), R23(i); ... 133 | R31(i), R32(i), R33(i)]; 134 | 135 | Wh = (R_last'*R_cur - eye(3)); 136 | W = 1/dt * Wh; 137 | gyro(i,1) = (W(3,2) - W(2,3))/2; 138 | gyro(i,2) = (W(1,3) - W(3,1))/2; 139 | gyro(i,3) = (W(2,1) - W(1,2))/2; 140 | end 141 | 142 | %% calibrate IMU data 143 | imusWithKukaCalibration; 144 | 145 | 146 | if (~use_comparisonAlgorithms) 147 | disp('To get reference algorithms, download their implementations from here:'); 148 | disp('http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/'); 149 | disp(' '); 150 | end 151 | 152 | %% start bias test computation (compute everything with test region using different biases) 153 | % test and computation is done only for InertiaLink data 154 | testBiases = ((0:0.5:7)*pi/180)'; 155 | %testBiases = ((0:0.5:1)*pi/180)'; 156 | 157 | yaw_DCM_RMSE = zeros(size(testBiases)); 158 | yaw_e1_RMSE = zeros(size(testBiases)); 159 | yaw_e2_RMSE = zeros(size(testBiases)); 160 | 161 | pitch_DCM_RMSE = zeros(size(testBiases)); 162 | pitch_e1_RMSE = zeros(size(testBiases)); 163 | pitch_e2_RMSE = zeros(size(testBiases)); 164 | 165 | roll_DCM_RMSE = zeros(size(testBiases)); 166 | roll_e1_RMSE = zeros(size(testBiases)); 167 | roll_e2_RMSE = zeros(size(testBiases)); 168 | 169 | acc2 = [acc2_x(StartIdx:StopIdx), acc2_y(StartIdx:StopIdx), acc2_z(StartIdx:StopIdx)]; 170 | gyro2_noBias = [w2_x(StartIdx:StopIdx), w2_y(StartIdx:StopIdx), w2_z(StartIdx:StopIdx)]; 171 | time = ILtime(StartIdx:StopIdx); 172 | deltaT = [0; time(2:end) - time(1:end-1)]; 173 | 174 | %resample Kuka reference data to InertiaLink time 175 | warning('off','interpolation:interpolation:noextrap'); 176 | yaw_in_IL = resample(timeseries(yaw, Ktime),time); 177 | yaw_in_IL = yaw_in_IL.Data; 178 | yaw_in_IL = yaw_in_IL - yaw_in_IL(1); %starting yaw from zero (same as IMU estimates) 179 | pitch_in_IL = resample(timeseries(pitch, Ktime),time); 180 | pitch_in_IL = pitch_in_IL.Data; 181 | roll_in_IL = resample(timeseries(roll, Ktime),time); 182 | roll_in_IL = roll_in_IL.Data; 183 | warning('on','interpolation:interpolation:noextrap'); 184 | 185 | disp('Starting to compute RMSEs using different biases'); 186 | 187 | %check if matlabpool is opened 188 | scheduler = findResource(); 189 | if (matlabpool('size') < scheduler.ClusterSize) 190 | if (matlabpool('size') > 0) 191 | matlabpool close; 192 | end 193 | 194 | matlabpool(scheduler.ClusterSize); 195 | end 196 | 197 | warning('off','MATLAB:mir_warning_maybe_uninitialized_temporary'); 198 | parfor i = 2:(length(testBiases)) 199 | disp(['iteration ' int2str(i) ', bias ' num2str(testBiases(i)*180/pi) ' deg/s']); 200 | 201 | % add constant bias for testing purposes 202 | addedGyroBias = testBiases(i); 203 | gyro2 = gyro2_noBias + addedGyroBias*ones(size(gyro2_noBias)); 204 | 205 | if (use_C_versions) 206 | [x_hist2, ypr_hist2, a_hist2, P_diag_hist2] = ... 207 | DCM_IMU_C(gyro2, acc2, deltaT); 208 | 209 | if (use_comparisonAlgorithms) 210 | quaternion1 = Madgwick_IMU_C(gyro2, acc2); 211 | quaternion2 = Mahony_IMU_C(gyro2, acc2); 212 | else 213 | quaternion1 = nan(size(gyro2,1),4); 214 | quaternion2 = nan(size(gyro2,1),4); 215 | end 216 | else 217 | DCM = DCM_IMU(); 218 | 219 | if (use_comparisonAlgorithms) 220 | addpath('quaternion_library'); % include quaternion library 221 | 222 | IMU1t = MadgwickAHRS('SamplePeriod', 1/150, 'Beta', 0.1); 223 | IMU2t = MahonyAHRS('SamplePeriod', 1/150, 'Kp', 0.5); 224 | 225 | quaternion1 = zeros(length(time), 4); 226 | quaternion2 = zeros(length(time), 4); 227 | else 228 | quaternion1 = nan(length(time), 4); 229 | quaternion2 = nan(length(time), 4); 230 | end 231 | 232 | x_hist2 = zeros(length(time), 6); 233 | ypr_hist2 = zeros(length(time), 3); 234 | P_diag_hist2 = zeros(length(time), 6); 235 | for t = 1:length(time) 236 | DCM.UpdateIMU(gyro2(t,:), acc2(t,:), deltaT(t)); % gyroscope units must be radians 237 | x_hist2(t, :) = DCM.state'; 238 | ypr_hist2(t, :) = [DCM.yaw, DCM.pitch, DCM.roll]; 239 | P_diag_hist2(t, :) = diag(DCM.P)'; 240 | 241 | if (use_comparisonAlgorithms) 242 | IMU1t.UpdateIMU(gyro2(t,:), acc2(t,:)); % gyroscope units must be radians 243 | quaternion1(t, :) = IMU1t.Quaternion; 244 | 245 | IMU2t.UpdateIMU(gyro2(t,:), acc2(t,:)); % gyroscope units must be radians 246 | quaternion2(t, :) = IMU2t.Quaternion; 247 | end 248 | end 249 | end 250 | 251 | 252 | % Plot algorithm output as Euler angles 253 | % The first and third Euler angles in the sequence (phi and psi) become 254 | % unreliable when the middle angles of the sequence (theta) approaches ~90 255 | % degrees. This problem commonly referred to as Gimbal Lock. 256 | % See: http://en.wikipedia.org/wiki/Gimbal_lock 257 | 258 | % use conjugate for sensor frame relative to Earth and convert to degrees. 259 | if (use_comparisonAlgorithms) 260 | euler1 = quatern2euler(quaternConj(quaternion1)) * (180/pi); 261 | euler2 = quatern2euler(quaternConj(quaternion2)) * (180/pi); 262 | else 263 | euler1 = nan(size(quaternion1,1),3); 264 | euler2 = nan(size(quaternion2,1),3); 265 | end 266 | 267 | % compute errors 268 | 269 | %yaw pitch roll errors (only InertiaLink data) 270 | ypr_hist2(:,1) = un_modulo(ypr_hist2(:,1), 2*pi); %remove 2pi jumps and make continuous 271 | ypr_hist2(:,1) = ypr_hist2(:,1) - ypr_hist2(1,1); %DCM 272 | 273 | euler1(:,3) = un_modulo(euler1(:,3), 360); %remove 2pi jumps and make continuous 274 | euler1(:,3) = euler1(:,3) - euler1(1,3); %Madgwick 275 | 276 | euler2(:,3) = un_modulo(euler2(:,3), 360); %remove 2pi jumps and make continuous 277 | euler2(:,3) = euler2(:,3) - euler2(1,3); %Mahony 278 | 279 | %yaw 280 | yaw_DCM_error = (ypr_hist2(:,1)-yaw_in_IL)*180/pi; 281 | yaw_DCM_RMSE(i) = sqrt(mean(yaw_DCM_error.^2)); 282 | yaw_e1_error = euler1(:,3) - yaw_in_IL*180/pi; 283 | yaw_e1_RMSE(i) = sqrt(mean(yaw_e1_error.^2)); 284 | yaw_e2_error = euler2(:,3) - yaw_in_IL*180/pi; 285 | yaw_e2_RMSE(i) = sqrt(mean(yaw_e2_error.^2)); 286 | 287 | %pitch 288 | pitch_DCM_error = (ypr_hist2(:,2)-pitch_in_IL)*180/pi; 289 | pitch_DCM_RMSE(i) = sqrt(mean(pitch_DCM_error.^2)); 290 | pitch_e1_error = euler1(:,2) - pitch_in_IL*180/pi; 291 | pitch_e1_RMSE(i) = sqrt(mean(pitch_e1_error.^2)); 292 | pitch_e2_error = euler2(:,2) - pitch_in_IL*180/pi; 293 | pitch_e2_RMSE(i) = sqrt(mean(pitch_e2_error.^2)); 294 | 295 | %roll 296 | roll_DCM_error = (ypr_hist2(:,3)-roll_in_IL)*180/pi; 297 | roll_DCM_RMSE(i) = sqrt(mean(roll_DCM_error.^2)); 298 | roll_e1_error = euler1(:,1) - roll_in_IL*180/pi; 299 | roll_e1_RMSE(i) = sqrt(mean(roll_e1_error.^2)); 300 | roll_e2_error = euler2(:,1) - roll_in_IL*180/pi; 301 | roll_e2_RMSE(i) = sqrt(mean(roll_e2_error.^2)); 302 | end 303 | warning('on','MATLAB:mir_warning_maybe_uninitialized_temporary'); 304 | %% plotting 305 | 306 | % compute same for smallest and some other index (for separate plotting) 307 | figIdx = 1; 308 | j_idx = [1 3]; 309 | for j = 1:2 310 | i = j_idx(j); 311 | 312 | disp(['iteration ' int2str(i) ', bias ' num2str(testBiases(i)*180/pi) ' deg/s']); 313 | 314 | % add constant bias for testing purposes 315 | addedGyroBias = testBiases(i); 316 | gyro2 = gyro2_noBias + addedGyroBias*ones(size(gyro2_noBias)); 317 | 318 | if (use_C_versions) 319 | [x_hist2, ypr_hist2, a_hist2, P_diag_hist2] = ... 320 | DCM_IMU_C(gyro2, acc2, deltaT); 321 | 322 | if (use_comparisonAlgorithms) 323 | quaternion1 = Madgwick_IMU_C(gyro2, acc2); 324 | quaternion2 = Mahony_IMU_C(gyro2, acc2); 325 | else 326 | quaternion1 = nan(size(gyro2,1),4); 327 | quaternion2 = nan(size(gyro2,1),4); 328 | end 329 | else 330 | DCM = DCM_IMU(); 331 | 332 | if (use_comparisonAlgorithms) 333 | addpath('quaternion_library'); % include quaternion library 334 | 335 | IMU1 = MadgwickAHRS('SamplePeriod', 1/150, 'Beta', 0.1); 336 | IMU2 = MahonyAHRS('SamplePeriod', 1/150, 'Kp', 0.5); 337 | 338 | quaternion1 = zeros(length(time), 4); 339 | quaternion2 = zeros(length(time), 4); 340 | else 341 | quaternion1 = nan(length(time), 4); 342 | quaternion2 = nan(length(time), 4); 343 | end 344 | 345 | x_hist2 = zeros(length(time), 6); 346 | ypr_hist2 = zeros(length(time), 3); 347 | P_diag_hist2 = zeros(length(time), 6); 348 | for t = 1:length(time) 349 | DCM.UpdateIMU(gyro2(t,:), acc2(t,:), deltaT(t)); % gyroscope units must be radians 350 | x_hist2(t, :) = DCM.state'; 351 | ypr_hist2(t, :) = [DCM.yaw, DCM.pitch, DCM.roll]; 352 | P_diag_hist2(t, :) = diag(DCM.P)'; 353 | 354 | if (use_comparisonAlgorithms) 355 | IMU1.UpdateIMU(gyro2(t,:), acc2(t,:)); % gyroscope units must be radians 356 | quaternion1(t, :) = IMU1.Quaternion; 357 | 358 | IMU2.UpdateIMU(gyro2(t,:), acc2(t,:)); % gyroscope units must be radians 359 | quaternion2(t, :) = IMU2.Quaternion; 360 | end 361 | end 362 | end 363 | 364 | % Plot algorithm output as Euler angles 365 | % The first and third Euler angles in the sequence (phi and psi) become 366 | % unreliable when the middle angles of the sequence (theta) approaches ~90 367 | % degrees. This problem commonly referred to as Gimbal Lock. 368 | % See: http://en.wikipedia.org/wiki/Gimbal_lock 369 | 370 | % use conjugate for sensor frame relative to Earth and convert to degrees. 371 | if (use_comparisonAlgorithms) 372 | euler1 = quatern2euler(quaternConj(quaternion1)) * (180/pi); 373 | euler2 = quatern2euler(quaternConj(quaternion2)) * (180/pi); 374 | else 375 | euler1 = nan(size(quaternion1,1),3); 376 | euler2 = nan(size(quaternion2,1),3); 377 | end 378 | 379 | % compute errors 380 | 381 | % yaw pitch roll errors (only InertiaLink data) 382 | ypr_hist2(:,1) = un_modulo(ypr_hist2(:,1), 2*pi); %remove 2pi jumps and make continuous 383 | ypr_hist2(:,1) = ypr_hist2(:,1) - ypr_hist2(1,1); %DCM 384 | 385 | euler1(:,3) = un_modulo(euler1(:,3), 360); %remove 2pi jumps and make continuous 386 | euler1(:,3) = euler1(:,3) - euler1(1,3); %Madgwick 387 | 388 | euler2(:,3) = un_modulo(euler2(:,3), 360); %remove 2pi jumps and make continuous 389 | euler2(:,3) = euler2(:,3) - euler2(1,3); %Mahony 390 | 391 | %yaw 392 | yaw_DCM_error = (ypr_hist2(:,1)-yaw_in_IL)*180/pi; 393 | yaw_DCM_RMSE(i) = sqrt(mean(yaw_DCM_error.^2)); 394 | yaw_e1_error = euler1(:,3) - yaw_in_IL*180/pi; 395 | yaw_e1_RMSE(i) = sqrt(mean(yaw_e1_error.^2)); 396 | yaw_e2_error = euler2(:,3) - yaw_in_IL*180/pi; 397 | yaw_e2_RMSE(i) = sqrt(mean(yaw_e2_error.^2)); 398 | 399 | %pitch 400 | pitch_DCM_error = (ypr_hist2(:,2)-pitch_in_IL)*180/pi; 401 | pitch_DCM_RMSE(i) = sqrt(mean(pitch_DCM_error.^2)); 402 | pitch_e1_error = euler1(:,2) - pitch_in_IL*180/pi; 403 | pitch_e1_RMSE(i) = sqrt(mean(pitch_e1_error.^2)); 404 | pitch_e2_error = euler2(:,2) - pitch_in_IL*180/pi; 405 | pitch_e2_RMSE(i) = sqrt(mean(pitch_e2_error.^2)); 406 | 407 | %roll 408 | roll_DCM_error = (ypr_hist2(:,3)-roll_in_IL)*180/pi; 409 | roll_DCM_RMSE(i) = sqrt(mean(roll_DCM_error.^2)); 410 | roll_e1_error = euler1(:,1) - roll_in_IL*180/pi; 411 | roll_e1_RMSE(i) = sqrt(mean(roll_e1_error.^2)); 412 | roll_e2_error = euler2(:,1) - roll_in_IL*180/pi; 413 | roll_e2_RMSE(i) = sqrt(mean(roll_e2_error.^2)); 414 | 415 | % plotting 416 | % Yaw, pitch and roll plot 417 | figure(figIdx); clf; 418 | subplot(3,1,1); 419 | yaw_dcm = ypr_hist2(:,1)*180/pi; 420 | pitch_dcm = ypr_hist2(:,2)*180/pi; 421 | roll_dcm = ypr_hist2(:,3)*180/pi; 422 | 423 | plot(time, yaw_dcm, 'b', time, euler1(:,3), 'g--', time, euler2(:,3), 'r:', time, yaw_in_IL*180/pi, 'k--', 'LineWidth', 1); 424 | set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times'); 425 | legend('DCM', 'Madgwick', 'Mahony', 'Reference', 'Location', legendLocation([yaw_dcm euler1(:,3) euler2(:,3)])); 426 | 427 | ylabel('yaw (deg)', 'FontSize', 12, 'FontName', 'Times'); 428 | title(['Euler angles with a bias of ' num2str(addedGyroBias*180/pi, '%.0f') ' deg/s'], 'FontSize', 14, 'FontName', 'Times'); 429 | set(gca, 'XLim', [time(1) time(end)]); 430 | 431 | subplot(3,1,2); 432 | plot(time, pitch_dcm, 'b', time, euler1(:,2), 'g--', time, euler2(:,2), 'r:', time, pitch_in_IL*180/pi, 'k--', 'LineWidth', 1); 433 | set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times'); 434 | ylabel('pitch (deg)', 'FontSize', 12, 'FontName', 'Times'); 435 | set(gca, 'XLim', [time(1) time(end)]); 436 | 437 | subplot(3,1,3); 438 | plot(time, roll_dcm, 'b', time, euler1(:,1), 'g--', time, euler2(:,1), 'r:', time, roll_in_IL*180/pi, 'k--', 'LineWidth', 1); 439 | set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times'); 440 | xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times'); 441 | ylabel('roll (deg)', 'FontSize', 12, 'FontName', 'Times'); 442 | set(gca, 'XLim', [time(1) time(end)]); 443 | 444 | set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]); 445 | set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]); 446 | set(gcf,'PaperPositionMode','auto'); 447 | saveas(gcf, ['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.fig']); 448 | print('-depsc','-tiff','-r300',['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.eps']) 449 | figIdx = figIdx + 1; 450 | 451 | % error plot 452 | figure(figIdx); clf; 453 | 454 | subplot(3,1,1); 455 | plot(time, yaw_DCM_error, 'b', time, yaw_e1_error, 'g--', time, yaw_e2_error, 'r:', 'LineWidth', 1); 456 | set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times'); 457 | legend('DCM', 'Madgwick', 'Mahony', 'Location', legendLocation([yaw_DCM_error yaw_e1_error yaw_e2_error])); 458 | ylabel('yaw error (deg)', 'FontSize', 12, 'FontName', 'Times'); 459 | title(['Errors to the reference measurement with a bias of ' num2str(addedGyroBias*180/pi, '%.0f') ' deg/s'], 'FontSize', 14, 'FontName', 'Times'); 460 | set(gca, 'XLim', [time(1) time(end)]); 461 | 462 | subplot(3,1,2); 463 | plot(time, pitch_DCM_error, 'b', time, pitch_e1_error, 'g--', time, pitch_e2_error, 'r:', 'LineWidth', 1); 464 | set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times'); 465 | ylabel('pitch error (deg)', 'FontSize', 12, 'FontName', 'Times'); 466 | set(gca, 'XLim', [time(1) time(end)]); 467 | 468 | subplot(3,1,3); 469 | plot(time, roll_DCM_error, 'b', time, roll_e1_error, 'g--', time, roll_e2_error, 'r:', 'LineWidth', 1); 470 | set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times'); 471 | xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times'); 472 | ylabel('roll error (deg)', 'FontSize', 12, 'FontName', 'Times'); 473 | set(gca, 'XLim', [time(1) time(end)]); 474 | 475 | set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]); 476 | set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]); 477 | set(gcf,'PaperPositionMode','auto'); 478 | saveas(gcf, ['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.fig']); 479 | print('-depsc','-tiff','-r300',['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.eps']) 480 | figIdx = figIdx + 1; 481 | 482 | % bias plot 483 | figure(figIdx); clf; 484 | 485 | x_sigma = sqrt(P_diag_hist2(:,4)); 486 | y_sigma = sqrt(P_diag_hist2(:,5)); 487 | z_sigma = sqrt(P_diag_hist2(:,6)); 488 | 489 | bias_x = x_hist2(:,4)*180/pi; 490 | bias_y = x_hist2(:,5)*180/pi; 491 | bias_z = x_hist2(:,6)*180/pi; 492 | 493 | plotLims = 1.25*[mean(x_sigma(1:floor(end/3))), mean(y_sigma(1:floor(end/3))), mean(z_sigma(1:floor(end/3)))]; 494 | 495 | subplot(3,1,1); 496 | plot([time(1) time(end)], [addedGyroBias addedGyroBias]*180/pi, 'k--', ... 497 | time, bias_x, 'b', time, (addedGyroBias-x_sigma)*180/pi, 'b-.', ... 498 | time, (addedGyroBias+x_sigma)*180/pi, 'b-.', 'LineWidth', 1, 'MarkerSize', markerSize); 499 | h = legend('Reference', 'Estimate', '1-sigma'); 500 | set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times'); 501 | 502 | ylabel('x_b_i_a_s (deg/s)', 'FontSize', 12, 'FontName', 'Times'); 503 | title('Bias estimates and 1-sigma distances of standard deviations', 'FontSize', 14, 'FontName', 'Times'); 504 | set(gca, 'XLim', [time(1) time(end)], 'YLim', [addedGyroBias-plotLims(1) addedGyroBias+plotLims(1)]*180/pi); 505 | 506 | subplot(3,1,2); 507 | plot([time(1) time(end)], [addedGyroBias addedGyroBias]*180/pi, 'k--', ... 508 | time, bias_y, 'b', time, (addedGyroBias-y_sigma)*180/pi, 'b-.', ... 509 | time, (addedGyroBias+y_sigma)*180/pi, 'b-.', 'LineWidth', 1, 'MarkerSize', markerSize); 510 | set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times'); 511 | 512 | ylabel('y_b_i_a_s (deg/s)', 'FontSize', 12, 'FontName', 'Times'); 513 | set(gca, 'XLim', [time(1) time(end)], 'YLim', [addedGyroBias-plotLims(2) addedGyroBias+plotLims(2)]*180/pi); 514 | 515 | subplot(3,1,3); 516 | plot([time(1) time(end)], [addedGyroBias addedGyroBias]*180/pi, 'k--', ... 517 | time, bias_z, 'b', time, (addedGyroBias-z_sigma)*180/pi, 'b-.', ... 518 | time, (addedGyroBias+z_sigma)*180/pi, 'b-.', 'LineWidth', 1, 'MarkerSize', markerSize); 519 | set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times'); 520 | 521 | xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times'); 522 | ylabel('z_b_i_a_s (deg/s)', 'FontSize', 12, 'FontName', 'Times'); 523 | set(gca, 'XLim', [time(1) time(end)], 'YLim', [addedGyroBias-plotLims(3) addedGyroBias+plotLims(3)]*180/pi); 524 | 525 | set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]); 526 | set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]); 527 | set(gcf,'PaperPositionMode','auto'); 528 | saveas(gcf, ['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.fig']); 529 | print('-depsc','-tiff','-r300',['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.eps']) 530 | figIdx = figIdx + 1; 531 | 532 | pause(0.2); 533 | end 534 | 535 | %% statistics plot 536 | figure(figIdx); clf; 537 | 538 | subplot(3,1,1); 539 | semilogy(testBiases*180/pi, yaw_DCM_RMSE, 'b*-', testBiases*180/pi, yaw_e1_RMSE, 'g^-', testBiases*180/pi, yaw_e2_RMSE, 'ro-', 'LineWidth', 1, 'MarkerSize', markerSize); 540 | set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times'); 541 | legend('DCM', 'Madgwick', 'Mahony', 'Location', 'SouthEast'); 542 | ylabel('yaw RMSE (deg)', 'FontSize', 12, 'FontName', 'Times'); 543 | title(['RMSEs as a function of gyroscope bias (' testName ')'], 'FontSize', 14, 'FontName', 'Times'); 544 | 545 | subplot(3,1,2); 546 | semilogy(testBiases*180/pi, pitch_DCM_RMSE, 'b*-', testBiases*180/pi, pitch_e1_RMSE, 'g^-', testBiases*180/pi, pitch_e2_RMSE, 'ro-', 'LineWidth', 1, 'MarkerSize', markerSize); 547 | set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times'); 548 | ylabel('pitch RMSE (deg)', 'FontSize', 12, 'FontName', 'Times'); 549 | 550 | subplot(3,1,3); 551 | semilogy(testBiases*180/pi, roll_DCM_RMSE, 'b*-', testBiases*180/pi, roll_e1_RMSE, 'g^-', testBiases*180/pi, roll_e2_RMSE, 'ro-', 'LineWidth', 1, 'MarkerSize', markerSize); 552 | set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times'); 553 | xlabel('Constant gyroscope bias (deg/s)', 'FontSize', 12, 'FontName', 'Times'); 554 | ylabel('roll RMSE (deg)', 'FontSize', 12, 'FontName', 'Times'); 555 | 556 | set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]); 557 | set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]); 558 | set(gcf,'PaperPositionMode','auto'); 559 | saveas(gcf, ['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.fig']); 560 | print('-depsc','-tiff','-r300',['figures/bias_fig_' testName(1:3) '_' int2str(figIdx) '.eps']) 561 | 562 | 563 | -------------------------------------------------------------------------------- /rootMeanSquaredErrors.m: -------------------------------------------------------------------------------- 1 | %============================================================================ 2 | % Copyright (C) 2014, Heikki Hyyti 3 | % 4 | % Permission is hereby granted, free of charge, to any person obtaining a 5 | % copy of this software and associated documentation files (the "Software"), 6 | % to deal in the Software without restriction, including without limitation 7 | % the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | % and/or sell copies of the Software, and to permit persons to whom the 9 | % Software is furnished to do so, subject to the following conditions: 10 | % 11 | % The above copyright notice and this permission notice shall be included in 12 | % all copies or substantial portions of the Software. 13 | % 14 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | % THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | % FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | % DEALINGS IN THE SOFTWARE. 21 | %============================================================================ 22 | 23 | function [vals, lags] = rootMeanSquaredErrors(data, ref, n_lags) 24 | lags = -n_lags:1:n_lags; 25 | vals = zeros(size(lags)); 26 | base_start = 1 + n_lags; 27 | base_stop = length(data) - n_lags; 28 | for i = 1:length(lags) 29 | start_idx = base_start + lags(i); 30 | stop_idx = base_stop + lags(i); 31 | vals(i) = sqrt(mean((data(start_idx:stop_idx) - ref(base_start:base_stop)).^2)); 32 | end 33 | end -------------------------------------------------------------------------------- /un_modulo.m: -------------------------------------------------------------------------------- 1 | %============================================================================ 2 | % Copyright (C) 2014, Heikki Hyyti 3 | % 4 | % Permission is hereby granted, free of charge, to any person obtaining a 5 | % copy of this software and associated documentation files (the "Software"), 6 | % to deal in the Software without restriction, including without limitation 7 | % the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | % and/or sell copies of the Software, and to permit persons to whom the 9 | % Software is furnished to do so, subject to the following conditions: 10 | % 11 | % The above copyright notice and this permission notice shall be included in 12 | % all copies or substantial portions of the Software. 13 | % 14 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | % THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | % FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | % DEALINGS IN THE SOFTWARE. 21 | %============================================================================ 22 | 23 | function out = un_modulo(in, modulus) 24 | out = zeros(size(in)); 25 | out(1,:) = in(1,:); 26 | offset = zeros(1, size(in,2)); 27 | for i = 2:size(in,1) 28 | diff_to_last = in(i,:) - in(i-1,:); 29 | shift_down = diff_to_last > (modulus/2); 30 | offset(shift_down) = offset(shift_down) - modulus; 31 | shift_up = diff_to_last < (-modulus/2); 32 | offset(shift_up) = offset(shift_up) + modulus; 33 | 34 | out(i,:) = offset + in(i,:); 35 | end 36 | end -------------------------------------------------------------------------------- /yawpitchroll.m: -------------------------------------------------------------------------------- 1 | %============================================================================ 2 | % Copyright (C) 2014, Heikki Hyyti 3 | % 4 | % Permission is hereby granted, free of charge, to any person obtaining a 5 | % copy of this software and associated documentation files (the "Software"), 6 | % to deal in the Software without restriction, including without limitation 7 | % the rights to use, copy, modify, merge, publish, distribute, sublicense, 8 | % and/or sell copies of the Software, and to permit persons to whom the 9 | % Software is furnished to do so, subject to the following conditions: 10 | % 11 | % The above copyright notice and this permission notice shall be included in 12 | % all copies or substantial portions of the Software. 13 | % 14 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 17 | % THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 19 | % FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 20 | % DEALINGS IN THE SOFTWARE. 21 | %============================================================================ 22 | 23 | function [y, p, r] = yawpitchroll(R11, R21, R31, R32, R33) 24 | y = atan2(R21, R11); 25 | p = asin(-R31); 26 | r = atan2(R32,R33); 27 | end --------------------------------------------------------------------------------