├── .gitignore ├── README.md ├── SimulationResults.pdf └── srcs ├── IMU0x2Dalpha.mat ├── IMU0x2Domega.mat ├── READ_ME.txt ├── accCostFunctLSQNONLIN.m ├── calibrate.m ├── calibrate_inemo.m ├── compute_time_interval_info_matrix.m ├── fromOmegaToQ.m ├── fromQtoR.m ├── gyroCostFunctLSQNONLIN.m ├── inemo calibration.txt ├── obtainComparableMatrix.m └── rotationRK4.m /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | bin/ 3 | lib/ 4 | msg_gen/ 5 | srv_gen/ 6 | msg/*Action.msg 7 | msg/*ActionFeedback.msg 8 | msg/*ActionGoal.msg 9 | msg/*ActionResult.msg 10 | msg/*Feedback.msg 11 | msg/*Goal.msg 12 | msg/*Result.msg 13 | msg/_*.py 14 | 15 | # Generated by dynamic reconfigure 16 | *.cfgc 17 | /cfg/cpp/ 18 | /cfg/*.py 19 | 20 | # Ignore generated docs 21 | *.dox 22 | *.wikidoc 23 | 24 | # eclipse stuff 25 | .project 26 | .cproject 27 | 28 | # qcreator stuff 29 | CMakeLists.txt.user 30 | 31 | srv/_*.py 32 | *.pcd 33 | *.pyc 34 | qtcreator-* 35 | *.user 36 | 37 | /planning/cfg 38 | /planning/docs 39 | /planning/src 40 | 41 | *~ 42 | 43 | # Emacs 44 | .#* 45 | 46 | # Catkin custom files 47 | CATKIN_IGNORE 48 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # imu_tk_matlab 2 | Matlab scripts of David Tedaldi's ICRA14 paper, a robust and easy to implement method for IMU calibration。 3 | 4 | It is a deterministic calibration method for a low cost IMU. The estimated parameters include biases, scale factor, and misalignment of the 3-axis accelerometer and gyroscope. 5 | 6 | The corresponding C++ implementation is [here](https://bitbucket.org/alberto_pretto/imu_tk). 7 | -------------------------------------------------------------------------------- /SimulationResults.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JzHuai0108/imu_tk_matlab/2b3880fd5daf18881d47b360036690d8854cec22/SimulationResults.pdf -------------------------------------------------------------------------------- /srcs/IMU0x2Dalpha.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JzHuai0108/imu_tk_matlab/2b3880fd5daf18881d47b360036690d8854cec22/srcs/IMU0x2Dalpha.mat -------------------------------------------------------------------------------- /srcs/IMU0x2Domega.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JzHuai0108/imu_tk_matlab/2b3880fd5daf18881d47b360036690d8854cec22/srcs/IMU0x2Domega.mat -------------------------------------------------------------------------------- /srcs/READ_ME.txt: -------------------------------------------------------------------------------- 1 | Running 'calibrate.m' it will be displayed on console the calibration parameter 2 | taken from Xsens™ MTi™ we use on our real tests followed by the estimated parameters 3 | we obtain with our method. 4 | 5 | 'IMU0x2Dalpha.mat' and 'IMU0x2Domega.mat' are the accelerometer raw data matrix and 6 | the gyroscope raw data matrix respectively. 7 | 8 | All the other file are functions used into 'calibrate.m': 9 | - 'accCostFunctLSQNONLIN.m' is the cost function used to optimize accelerometer 10 | sensor error model parameters; 11 | - 'gyroCostFunctLSQNONLIN.m' is the cost function used to optimize gyroscope 12 | sensor error model parameters; 13 | - 'rotationRK4.m' is the implementation of the quaternion kinematics Runge-Kutta 14 | 4th order integration algorithm; 15 | - 'fromOmegaToQ. m' compute the unit quaternion q associated to a three-dimensional 16 | angular velocity and a period of time t; 17 | - 'fromQtoR.m' compute the rotation matrix associated to a unit quaternion q; 18 | - 'obtainComparabelMatrix.m' compute the transformation that permit to compare the 19 | estimated matices to the ones given in the datasheet. -------------------------------------------------------------------------------- /srcs/accCostFunctLSQNONLIN.m: -------------------------------------------------------------------------------- 1 | %% accCostFunctLSQNONLIN.m 2 | 3 | function [ res_vector ] = accCostFunctLSQNONLIN( E, a_hat, magnitude) 4 | 5 | misalignmentMatrix = [1, -E(1), E(2); 0, 1, -E(3); 0, 0, 1]; 6 | scalingMtrix = diag([E(4), E(5), E(6)]); 7 | 8 | a_bar = misalignmentMatrix*scalingMtrix*(a_hat) - (diag([E(7), E(8), E(9)])*ones(3, size(a_hat,2))); 9 | 10 | % Magnitude taken from tables 11 | if(nargin<3) 12 | magnitude = 9.81744; 13 | end 14 | residuals = zeros(length(a_bar(1,:)), 1); 15 | 16 | for i = 1:length(a_bar(1,:)) 17 | residuals(i,1) = magnitude^2 - (a_bar(1,i)^2 + a_bar(2,i)^2 + a_bar(3,i)^2); 18 | end 19 | 20 | res_vector = residuals; 21 | 22 | end -------------------------------------------------------------------------------- /srcs/calibrate.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | close all; 3 | clc; 4 | 5 | % Importing data 6 | IMU0x2Dalpha = importdata('IMU0x2Dalpha.mat'); 7 | IMU0x2Domega = importdata('IMU0x2Domega.mat'); 8 | 9 | time = IMU0x2Domega(:,1)'; 10 | 11 | intervals = zeros(1, length(time)); 12 | intervals(1) = ((time(2) - time(1))/2) + time(1)/2; 13 | intervals(2:length(intervals)) = time(2:length(time))-time(1:length(time)-1); 14 | 15 | total_sample = length(IMU0x2Domega(:,1)); 16 | 17 | offset_acc_x = 33123; 18 | offset_acc_y = 33276; 19 | offset_acc_z = 32360; 20 | offset_gyro_x = 32768; 21 | offset_gyro_y = 32466; 22 | offset_gyro_z = 32485; 23 | 24 | a_xp = IMU0x2Dalpha(:,2)' - offset_acc_x*ones(1,total_sample); 25 | a_yp = IMU0x2Dalpha(:,3)' - offset_acc_y*ones(1,total_sample); 26 | a_zp = IMU0x2Dalpha(:,4)' - offset_acc_z*ones(1,total_sample); 27 | omega_x = IMU0x2Domega(:,2)' - offset_gyro_x*ones(1,total_sample); 28 | omega_y = IMU0x2Domega(:,3)' - offset_gyro_y*ones(1,total_sample); 29 | omega_z = IMU0x2Domega(:,4)' - offset_gyro_z*ones(1,total_sample); 30 | 31 | 32 | 33 | %% Static State Statistical Filter 34 | 35 | var_3D = (var(a_xp(1:3000))^2 + var(a_yp(1:3000))^2 + var(a_zp(1:3000))^2); 36 | 37 | w_d = 101; % Window's dimension 38 | 39 | normal_x = zeros(1, total_sample); 40 | normal_y = zeros(1, total_sample); 41 | normal_z = zeros(1, total_sample); 42 | 43 | half_w_d = floor(w_d/2); 44 | 45 | % inizialize integral 46 | integral_x = sum(a_xp(1:w_d)*0.01); 47 | integral_y = sum(a_yp(1:w_d)*0.01); 48 | integral_z = sum(a_zp(1:w_d)*0.01); 49 | 50 | for i = (half_w_d + 1):total_sample - (half_w_d + 1) 51 | 52 | normal_x(i) = var(a_xp(i - half_w_d:i + half_w_d)); 53 | normal_y(i) = var(a_yp(i - half_w_d:i + half_w_d)); 54 | normal_z(i) = var(a_zp(i - half_w_d:i + half_w_d)); 55 | 56 | end 57 | 58 | s_square = (normal_x.^2 + normal_y.^2 + normal_z.^2); 59 | 60 | plot(s_square); 61 | 62 | s_filter = zeros(1, total_sample); 63 | 64 | %% Cycle used to individuate the optimal threshold 65 | 66 | max_times_the_var = 10; 67 | 68 | res_norm_vector = zeros(9 + 1 + 1,max_times_the_var); 69 | 70 | for times_the_var = 1:max_times_the_var 71 | 72 | for i = half_w_d:total_sample - (half_w_d + 1) 73 | 74 | if s_square(i) < times_the_var*var_3D 75 | 76 | s_filter(i) = 1; 77 | 78 | end 79 | 80 | end 81 | 82 | 83 | filter = s_filter; 84 | l = 1; 85 | QS_time_interval_info_matrix = zeros(1, 1 + 1 + 1); 86 | samples = 0; 87 | start = 0; 88 | 89 | falg = 0; 90 | 91 | if filter(1) == 0 92 | 93 | flag = 0; 94 | 95 | else 96 | 97 | flag = 1; 98 | start = 1; 99 | 100 | end 101 | 102 | % cycle to determine the QS_time_interval_info_matrix 103 | for i = 1:length(filter) 104 | 105 | if flag == 0 && filter(i) == 0 106 | 107 | 108 | 109 | elseif flag == 1 && filter(i) == 1 110 | 111 | samples = samples + 1; 112 | 113 | elseif flag == 1 && filter(i) == 0 114 | 115 | QS_time_interval_info_matrix(l, 1:3) = [start, i - 1, samples]; 116 | l = l + 1; 117 | flag = 0; 118 | 119 | elseif flag == 0 && filter(i) == 1 120 | 121 | start = i; 122 | samples = 1; 123 | flag = 1; 124 | 125 | end 126 | 127 | end 128 | 129 | % data selection - accelerometer 130 | qsTime = 1; 131 | sample_period = 0.01; 132 | num_samples = qsTime/sample_period; 133 | 134 | signal = [a_xp;a_yp; a_zp]; 135 | 136 | selected_data = zeros(3, 1); 137 | l = 1; 138 | 139 | 140 | for j = 1:length(QS_time_interval_info_matrix(:,1)) 141 | 142 | if QS_time_interval_info_matrix(j,3) < num_samples 143 | 144 | 145 | 146 | else 147 | 148 | selection_step = floor(QS_time_interval_info_matrix(j,3)/num_samples); 149 | 150 | for i = 1:(num_samples - 1) 151 | 152 | selected_data(1:3, l) = signal(1:3, QS_time_interval_info_matrix(j, 1) + (i - 1)*selection_step); 153 | l = l + 1; 154 | 155 | end 156 | 157 | selected_data(1:3, l) = signal(1:3, QS_time_interval_info_matrix(j, 2)); 158 | l = l + 1; 159 | 160 | end 161 | 162 | end 163 | 164 | % minimization 165 | selectedAccData = selected_data; 166 | 167 | theta_pr = [0, 0, 0, 0, 0, 0, 0, 0, 0]; 168 | 169 | ObjectiveFunction = @(theta_pr) accCostFunctLSQNONLIN(theta_pr, selectedAccData); 170 | options = optimset('MaxFunEvals', 150000, 'MaxIter', 6000, 'TolFun', 10^(-10)); 171 | 172 | [theta_pr rsnorm] = lsqnonlin(ObjectiveFunction, theta_pr, [], [], options); 173 | 174 | res_norm_vector(:,times_the_var) = [theta_pr';rsnorm;times_the_var*var_3D]; 175 | 176 | 177 | end 178 | 179 | vec = res_norm_vector(10,:); 180 | 181 | z=find(vec==min(min(vec))); 182 | 183 | threshold_opt = res_norm_vector(11,z); 184 | 185 | theta_pr_opt = res_norm_vector(1:9,z)'; 186 | 187 | estimated_misalignmentMatrix = [1, -theta_pr_opt(1), theta_pr_opt(2); 0, 1, -theta_pr_opt(3); 0, 0, 1]; 188 | estimated_scalingMatrix = diag([theta_pr_opt(4), theta_pr_opt(5), theta_pr_opt(6)]); 189 | estimated_biasVector = [theta_pr_opt(7); theta_pr_opt(8); theta_pr_opt(9)]; 190 | 191 | s_filter = zeros(1, total_sample); 192 | 193 | for i = half_w_d:total_sample - (half_w_d + 1) 194 | 195 | if s_square(i) < threshold_opt 196 | 197 | s_filter(i) = 1; 198 | 199 | end 200 | 201 | end 202 | 203 | figure 204 | plot(a_xp) 205 | hold on 206 | plot(a_yp, 'red') 207 | plot(a_zp, 'green') 208 | plot(5000*s_filter, 'black') 209 | 210 | 211 | %-------------------------------------------------------------------------% 212 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 213 | %%% GYROSCOPE BIAS REMUVAL %%% 214 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 215 | 216 | % QS filter for the first static region individuation 217 | init_long_qs_interval_start = 0; 218 | init_long_qs_interval_end = 0; 219 | flag_is_first_long_static_interval = 1; 220 | 221 | for i=1:total_sample 222 | 223 | if s_filter(i) == 0 && flag_is_first_long_static_interval == 1 224 | 0; 225 | elseif s_filter(i) == 1 && flag_is_first_long_static_interval == 1 226 | init_long_qs_interval_start = i; 227 | flag_is_first_long_static_interval = 2; 228 | elseif s_filter(i) == 1 && flag_is_first_long_static_interval == 2 229 | elseif s_filter(i) == 0 && flag_is_first_long_static_interval == 2 230 | init_long_qs_interval_end = i; 231 | break 232 | end 233 | end 234 | 235 | figure 236 | plot(omega_x); 237 | hold on 238 | plot(omega_y, 'red'); 239 | plot(omega_z, 'green'); 240 | 241 | estimate_bias_x = mean(omega_x(init_long_qs_interval_start:init_long_qs_interval_end)); 242 | estimate_bias_y = mean(omega_y(init_long_qs_interval_start:init_long_qs_interval_end)); 243 | estimate_bias_z = mean(omega_z(init_long_qs_interval_start:init_long_qs_interval_end)); 244 | 245 | omega_x = omega_x - estimate_bias_x; 246 | omega_y = omega_y - estimate_bias_y; 247 | omega_z = omega_z - estimate_bias_z; 248 | 249 | figure 250 | plot(omega_x); 251 | hold on 252 | plot(omega_y, 'red'); 253 | plot(omega_z, 'green'); 254 | disp(estimated_biasVector); % accelerator bias vector 255 | disp(estimated_scalingMatrix); % acc 256 | disp(estimated_misalignmentMatrix); % acc 257 | disp([estimate_bias_x, estimate_bias_y, estimate_bias_z]); % gyro 258 | %-------------------------------------------------------------------------% 259 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 260 | %%% GYROSCOPE MINIMIZATION %%% 261 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 262 | 263 | estimated_acc_misalignmentMatrix = estimated_misalignmentMatrix; 264 | estimated_acc_scalingMatrix = estimated_scalingMatrix; 265 | estimated_acc_biasVector = estimated_biasVector; 266 | % Calibrating acceleromter data 267 | calib_acc = estimated_acc_misalignmentMatrix*estimated_acc_scalingMatrix*[a_xp;a_yp;a_zp] - (diag(estimated_acc_biasVector)*ones(3, length(a_xp))); 268 | a_xp_calib = calib_acc(1,:); 269 | a_yp_calib = calib_acc(2,:); 270 | a_zp_calib = calib_acc(3,:); 271 | 272 | filter = s_filter; 273 | l = 1; 274 | QS_time_interval_info_matrix = zeros(1 + 1 + 1 + 3, 1); 275 | 276 | samples = 0; 277 | start = 0; 278 | % Inizializzazione flag 279 | falg = 0; 280 | if filter(1) == 0 281 | flag = 0; 282 | else 283 | flag = 1; 284 | start = 1; 285 | end 286 | % cycle 287 | for i = 1:length(filter) 288 | if flag == 0 && filter(i) == 0 289 | 0; % do nothing 290 | elseif flag == 1 && filter(i) == 1 291 | samples = samples + 1; 292 | elseif flag == 1 && filter(i) == 0 293 | QS_time_interval_info_matrix(1:3, l) = [start; i - 1; samples]; 294 | l = l + 1; 295 | flag = 0; 296 | elseif flag == 0 && filter(i) == 1 297 | start = i; 298 | samples = 1; 299 | flag = 1; 300 | end 301 | end 302 | 303 | 304 | num_samples = qsTime/sample_period; 305 | 306 | signal = [a_xp_calib;a_yp_calib; a_zp_calib]; 307 | 308 | selected_data = zeros(3, 1); %% 1xN vector, where N is the number of selected data 309 | l = 1; 310 | 311 | for g = 1:length(QS_time_interval_info_matrix(1,:)) 312 | 313 | selected_acc_data = zeros(3,1); 314 | selection_step = floor(QS_time_interval_info_matrix(3,g)/num_samples); 315 | 316 | for i = 1:(num_samples - 1) 317 | 318 | selected_acc_data(1:3, i) = signal(1:3, QS_time_interval_info_matrix(1, g) + (i - 1)*selection_step); 319 | 320 | end 321 | 322 | selected_data(1:3, num_samples) = signal(1:3, QS_time_interval_info_matrix(2, g)); 323 | QS_time_interval_info_matrix(4:6, l) = mean(selected_acc_data, 2); 324 | l = l + 1; 325 | 326 | end 327 | 328 | QS_time_interval_calib_info_matrix = QS_time_interval_info_matrix; 329 | 330 | % Minimizing LSQNONLIN 331 | theta_pr_gyro = [1/6258,0,0,0,1/6258,0,0,0,1/6258]; 332 | option = optimset('TolX', 10^-7, 'TolFun' , 10^-6, 'MaxFunEvals', 400); 333 | theta_pr_gyro = lsqnonlin(@(theta_pr_gyro) gyroCostFunctLSQNONLIN(theta_pr_gyro, QS_time_interval_calib_info_matrix, omega_x, omega_y, omega_z), theta_pr_gyro, [], [], option); 334 | 335 | misal_matrix = [1, theta_pr_gyro(2), theta_pr_gyro(3); theta_pr_gyro(4), 1, theta_pr_gyro(6); theta_pr_gyro(7), theta_pr_gyro(8), 1]; 336 | scale_matrix = [theta_pr_gyro(1), 0, 0; 0, theta_pr_gyro(5), 0; 0, 0, theta_pr_gyro(9)]; 337 | 338 | DS_a_scale = diag([415, 413, 415]); 339 | DS_a_misal = [1.00, 0.00, -0.01; 0.01, 1.00, 0.01; 0.02, 0.01, 1.00]; 340 | DS_g_scale = diag([4778, 4758, 4766]); 341 | DS_g_misal = [1.00, -0.01, -0.02; 0.00, 1.00, 0.04; -0.01, 0.01, 1.00]; 342 | disp(scale_matrix); % gyro 343 | disp(misal_matrix); % gyro 344 | 345 | disp(['Accelerometer''','s Scaling Matrix taken from datasheet:']); 346 | disp(DS_a_scale); 347 | disp(['Accelerometer''','s Misalignment Matrix taken from datasheet:']); 348 | format bank 349 | disp(DS_a_misal); 350 | disp(['Gyroscope''','s Scaling Matrix taken from datasheet:']); 351 | format short 352 | disp(DS_g_scale); 353 | disp(['Gyroscope''','s Misalignment Matrix taken from datasheet:']); 354 | format bank 355 | disp(DS_g_misal); 356 | disp('-----------------------------------------------------------------') 357 | 358 | format short 359 | [comp_a_scale, comp_a_misal, comp_g_scale, comp_g_misal] = ... 360 | obtainComparableMatrix(estimated_scalingMatrix, estimated_misalignmentMatrix, scale_matrix, misal_matrix); 361 | 362 | disp(['Accelerometer''','s Estimated Scaling Matrix:']); 363 | disp(comp_a_scale); 364 | disp(['Accelerometer''','s Estimated Misalignment Matrix:']); 365 | disp(comp_a_misal); 366 | disp(['Gyroscope''','s Estimated Scaling Matrix:']); 367 | disp(comp_g_scale); 368 | disp(['Gyroscope''','s Estimated Misalignment Matrix:']); 369 | disp(comp_g_misal); -------------------------------------------------------------------------------- /srcs/calibrate_inemo.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | close all; 3 | clc; 4 | experim=1; 5 | format longg 6 | % Importing data 7 | switch (experim) 8 | case 0 9 | % David Tedaldi's setting for Xsens 10 | IMU0x2Dalpha = importdata('IMU0x2Dalpha.mat'); 11 | IMU0x2Domega = importdata('IMU0x2Domega.mat'); 12 | % gravity magnitude at the experiment site 13 | magnitude = 9.81744; 14 | % the accelerometer calibration parameters initial values 15 | theta_pr = [0, 0, 0, 0, 0, 0, 0, 0, 0]; 16 | % gyro calibration parameter settings 17 | theta_pr_gyro = [1/6258,0,0,0,1/6258,0,0,0,1/6258]; 18 | % values from Datasheet for Xsens 19 | DS_a_scale = diag([415, 413, 415]); 20 | DS_a_misal = [1.00, 0.00, -0.01; 0.01, 1.00, 0.01; 0.02, 0.01, 1.00]; 21 | DS_g_scale = diag([4778, 4758, 4766]); 22 | DS_g_misal = [1.00, -0.01, -0.02; 0.00, 1.00, 0.04; -0.01, 0.01, 1.00]; 23 | % offset used by Xsens 24 | offset_acc_x = 33123; 25 | offset_acc_y = 33276; 26 | offset_acc_z = 32360; 27 | offset_gyro_x = 32768; 28 | offset_gyro_y = 32466; 29 | offset_gyro_z = 32485; 30 | % device characteristics of Xsens 31 | freq=100; 32 | Tinit=30; 33 | case 1 34 | % Huai{ iNEMO data collected on June 19 2014 has a Tinit 112 sec. twait is 35 | % approximately 3 secs. 112 sec is reasonable beacuse in Tedaldi's 36 | % experiment, they found 50 s is good for 100HZ IMU. INEMO is 50HZ, so I 37 | % choose some value around 100 sec as Tinit. By Allan 38 | % variance analysis as done in Tedaldi's paper, 2014, from 50 to 39 | % 120 second the allan variance doesnot change much 40 | chunk=load('H:\OpenShoe-Matlab-Implementation\PoseKinIMU\keepme19062014111706.tsv'); 41 | chunk(:,1)=chunk(:,1)/1000; 42 | chunk(:,6)=-chunk(:,6); % wrong y sign 43 | IMU0x2Dalpha = chunk(:,1:4); 44 | IMU0x2Domega = chunk(:,[1,5:7]); 45 | % gravity magnitude at the experiment site 46 | magnitude = 9.8008; 47 | % the accelerometer calibration parameters initial values 48 | theta_pr = [0, 0, 0, 9.8/1000, 9.8/1000, 9.8/1000, -0.2617, 0.3032, 1.1519]; 49 | % bound=0.2; % artifically 0.2 gives better results 50 | % lb=[-.5, -.5, -.5, 9.8/2000, 9.8/2000, 9.8/2000, -bound, -bound, -bound]; % lower bound 51 | % ub=[.5, .5, .5, 9.8/500, 9.8/500, 9.8/500, bound, bound, bound]; % upper bound 52 | % gyro calibration parameter settings 53 | theta_pr_gyro = [pi/180,0,0,0,pi/180,0,0,0,pi/180]; 54 | 55 | % my supposed value for iNemo 56 | DS_a_scale = diag([1000/9.8, 1000/9.8, 1000/9.8]); 57 | DS_a_misal = [1.00, 0.00, 0.0; 0.0, 1.00, 0.0; 0.0, 0.0, 1.00]; 58 | DS_g_scale = diag([180/pi, 180/pi, 180/pi]); 59 | DS_g_misal = [1.00, 0.0, 0.0; 0.00, 1.00, 0.0; 0.0, 0.0, 1.00]; 60 | 61 | % offset for iNemo 62 | offset_acc_x = 0; 63 | offset_acc_y = 0; 64 | offset_acc_z = 0; 65 | offset_gyro_x = 0; 66 | offset_gyro_y = 0; 67 | offset_gyro_z = 0; 68 | % characteristics of iNemo 69 | freq=50; % how many samples we have in one second 70 | Tinit= 110; % how long is the initial static period 71 | end 72 | % } Huai 73 | 74 | time = IMU0x2Domega(:,1)'; 75 | 76 | intervals = zeros(1, length(time)); 77 | intervals(1) = ((time(2) - time(1))/2) + time(1)/2; 78 | intervals(2:length(intervals)) = time(2:length(time))-time(1:length(time)-1); 79 | 80 | total_sample = length(IMU0x2Domega(:,1)); 81 | 82 | a_xp = IMU0x2Dalpha(:,2)' - offset_acc_x*ones(1,total_sample); 83 | a_yp = IMU0x2Dalpha(:,3)' - offset_acc_y*ones(1,total_sample); 84 | a_zp = IMU0x2Dalpha(:,4)' - offset_acc_z*ones(1,total_sample); 85 | omega_x = IMU0x2Domega(:,2)' - offset_gyro_x*ones(1,total_sample); 86 | omega_y = IMU0x2Domega(:,3)' - offset_gyro_y*ones(1,total_sample); 87 | omega_z = IMU0x2Domega(:,4)' - offset_gyro_z*ones(1,total_sample); 88 | 89 | %% Static State Statistical Filter 90 | range=freq*Tinit; % what is the largest index for a sample belong to the initial static period 91 | var_3D = (var(a_xp(1:range))^2 + var(a_yp(1:range))^2 + var(a_zp(1:range))^2); 92 | w_d= freq+1; % Window's dimension 93 | normal_x = zeros(1, total_sample); 94 | normal_y = zeros(1, total_sample); 95 | normal_z = zeros(1, total_sample); 96 | 97 | half_w_d = floor(w_d/2); 98 | 99 | % inizialize integral 100 | integral_x = sum(a_xp(1:w_d)/freq); 101 | integral_y = sum(a_yp(1:w_d)/freq); 102 | integral_z = sum(a_zp(1:w_d)/freq); 103 | 104 | for i = (half_w_d + 1):total_sample - (half_w_d + 1) 105 | 106 | normal_x(i) = var(a_xp(i - half_w_d:i + half_w_d)); 107 | normal_y(i) = var(a_yp(i - half_w_d:i + half_w_d)); 108 | normal_z(i) = var(a_zp(i - half_w_d:i + half_w_d)); 109 | 110 | end 111 | 112 | s_square = (normal_x.^2 + normal_y.^2 + normal_z.^2); 113 | figure 114 | plot(s_square); 115 | title('s^2, variance in a window'); 116 | 117 | s_filter = zeros(1, total_sample); 118 | 119 | %% Cycle used to individuate the optimal threshold 120 | 121 | max_times_the_var = 20; 122 | 123 | res_norm_vector = zeros(9 + 1 + 1,max_times_the_var); 124 | 125 | for times_the_var = 1:max_times_the_var 126 | for i = half_w_d:total_sample - (half_w_d + 1) 127 | 128 | if s_square(i) < times_the_var*var_3D 129 | s_filter(i) = 1; 130 | end 131 | end 132 | [QS_time_interval_info_matrix, s_filter]=compute_time_interval_info_matrix(s_filter, freq); 133 | % data selection - accelerometer 134 | qsTime = 1; 135 | sample_period =1/freq; 136 | num_samples = qsTime/sample_period; 137 | 138 | signal = [a_xp;a_yp; a_zp]; 139 | 140 | selected_data = zeros(3, 1); 141 | l = 1; 142 | 143 | % select static samples with a step of selection_step 144 | for j = 1:length(QS_time_interval_info_matrix(:,1)) 145 | 146 | if QS_time_interval_info_matrix(j,3) < num_samples 147 | % do nothing when there is a mis detection of static period 148 | else 149 | selection_step = floor(QS_time_interval_info_matrix(j,3)/num_samples); % interval between static samples 150 | for i = 1:(num_samples - 1) 151 | selected_data(1:3, l) = signal(1:3, QS_time_interval_info_matrix(j, 1) + (i - 1)*selection_step); 152 | l = l + 1; 153 | end 154 | selected_data(1:3, l) = signal(1:3, QS_time_interval_info_matrix(j, 2)); 155 | l = l + 1; 156 | 157 | end 158 | 159 | end 160 | % minimization 161 | selectedAccData = selected_data; 162 | % Huai { 163 | ObjectiveFunction = @(theta_pr) accCostFunctLSQNONLIN(theta_pr, selectedAccData, magnitude); 164 | % options = optimset('MaxFunEvals', 150000, 'MaxIter', 6000, 165 | % 'TolFun', 10^(-10)); % setting by Tedaldi 166 | options = optimset('MaxFunEvals', 2000, 'MaxIter', 600, 'TolFun', 1e-6); % these parameters typically do not make much difference 167 | if(exist('lb', 'var')) 168 | [theta_pr rsnorm] = lsqnonlin(ObjectiveFunction, theta_pr, lb,ub, options); 169 | else 170 | [theta_pr rsnorm] = lsqnonlin(ObjectiveFunction, theta_pr, [],[], options); 171 | end 172 | % Huai } 173 | res_norm_vector(:,times_the_var) = [theta_pr';rsnorm;times_the_var*var_3D]; 174 | end 175 | 176 | vec = res_norm_vector(10,:); 177 | z=find(vec==min(vec)); 178 | % Huai { 179 | z=15; % from my observation, 8 is the best value to distinguish static session in my June 6 2014 iNemo dataset 180 | % } Huai 181 | threshold_opt = res_norm_vector(11,z); 182 | 183 | theta_pr_opt = res_norm_vector(1:9,z)'; 184 | 185 | estimated_misalignmentMatrix = [1, -theta_pr_opt(1), theta_pr_opt(2); 0, 1, -theta_pr_opt(3); 0, 0, 1]; 186 | estimated_scalingMatrix = diag([theta_pr_opt(4), theta_pr_opt(5), theta_pr_opt(6)]); 187 | estimated_biasVector = [theta_pr_opt(7); theta_pr_opt(8); theta_pr_opt(9)]; 188 | 189 | s_filter = zeros(1, total_sample); 190 | for i = half_w_d:total_sample - (half_w_d + 1) 191 | if s_square(i) < threshold_opt 192 | s_filter(i) = 1; 193 | end 194 | end 195 | [QS_time_interval_info_matrix, s_filter]=compute_time_interval_info_matrix(s_filter, freq); 196 | figure 197 | plot(a_xp) 198 | hold on 199 | plot(a_yp, 'red') 200 | plot(a_zp, 'green') 201 | plot(floor(max(a_zp)/2)*s_filter, 'black') 202 | title('specific force in x, y, z and the dynamic session with value 0'); 203 | legend('ax', 'ay', 'az', 'dynamic'); 204 | 205 | %-------------------------------------------------------------------------% 206 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 207 | %%% GYROSCOPE BIAS REMUVAL %%% 208 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 209 | 210 | % QS filter for the first static region individuation 211 | init_long_qs_interval_start = 0; 212 | init_long_qs_interval_end = 0; 213 | flag_is_first_long_static_interval = 1; 214 | 215 | for i=1:total_sample 216 | 217 | if s_filter(i) == 0 && flag_is_first_long_static_interval == 1 218 | 0; 219 | elseif s_filter(i) == 1 && flag_is_first_long_static_interval == 1 220 | init_long_qs_interval_start = i; 221 | flag_is_first_long_static_interval = 2; 222 | elseif s_filter(i) == 1 && flag_is_first_long_static_interval == 2 223 | elseif s_filter(i) == 0 && flag_is_first_long_static_interval == 2 224 | init_long_qs_interval_end = i; 225 | break 226 | end 227 | end 228 | 229 | figure 230 | plot(omega_x); 231 | hold on 232 | plot(omega_y, 'red'); 233 | plot(omega_z, 'green'); 234 | title('raw angular rates in x, y, z'); 235 | legend('x', 'y', 'z'); 236 | estimate_bias_x = mean(omega_x(init_long_qs_interval_start:init_long_qs_interval_end)); 237 | estimate_bias_y = mean(omega_y(init_long_qs_interval_start:init_long_qs_interval_end)); 238 | estimate_bias_z = mean(omega_z(init_long_qs_interval_start:init_long_qs_interval_end)); 239 | 240 | omega_x = omega_x - estimate_bias_x; 241 | omega_y = omega_y - estimate_bias_y; 242 | omega_z = omega_z - estimate_bias_z; 243 | 244 | figure 245 | plot(omega_x); 246 | hold on 247 | plot(omega_y, 'red'); 248 | plot(omega_z, 'green'); 249 | title('calibrated angular rates in x, y, z'); 250 | legend('x', 'y', 'z'); 251 | % Huai { 252 | disp(estimated_biasVector); % accelerator bias vector 253 | disp(estimated_scalingMatrix); % acc 254 | disp(estimated_misalignmentMatrix); % acc 255 | disp([estimate_bias_x, estimate_bias_y, estimate_bias_z]); % gyro 256 | % Huai} 257 | 258 | %-------------------------------------------------------------------------% 259 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 260 | %%% GYROSCOPE MINIMIZATION %%% 261 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 262 | 263 | estimated_acc_misalignmentMatrix = estimated_misalignmentMatrix; 264 | estimated_acc_scalingMatrix = estimated_scalingMatrix; 265 | estimated_acc_biasVector = estimated_biasVector; 266 | % Calibrating acceleromter data 267 | calib_acc = estimated_acc_misalignmentMatrix*estimated_acc_scalingMatrix*[a_xp;a_yp;a_zp] - (diag(estimated_acc_biasVector)*ones(3, length(a_xp))); 268 | a_xp_calib = calib_acc(1,:); 269 | a_yp_calib = calib_acc(2,:); 270 | a_zp_calib = calib_acc(3,:); 271 | 272 | num_samples = qsTime/sample_period; 273 | 274 | signal = [a_xp_calib;a_yp_calib; a_zp_calib]; 275 | 276 | selected_data = zeros(3, 1); %% 3xN vector, where N is the number of selected data 277 | l = 1; 278 | QS_time_interval_info_matrix=[QS_time_interval_info_matrix'; zeros(size(QS_time_interval_info_matrix, 2), size(QS_time_interval_info_matrix, 1))]; 279 | for g = 1:length(QS_time_interval_info_matrix(1,:)) 280 | selected_acc_data = zeros(3,1); 281 | selection_step = floor(QS_time_interval_info_matrix(3,g)/num_samples); 282 | 283 | for i = 1:(num_samples - 1) 284 | selected_acc_data(1:3, i) = signal(1:3, QS_time_interval_info_matrix(1, g) + (i - 1)*selection_step); 285 | end 286 | selected_data(1:3, num_samples) = signal(1:3, QS_time_interval_info_matrix(2, g)); 287 | QS_time_interval_info_matrix(4:6, l) = mean(selected_acc_data, 2); % the mean gravity vector in this static session 288 | l = l + 1; 289 | end 290 | QS_time_interval_calib_info_matrix = QS_time_interval_info_matrix; 291 | % Minimizing LSQNONLIN 292 | 293 | option = optimset('TolX', 10^-7, 'TolFun' , 10^-6, 'MaxFunEvals', 400); 294 | theta_pr_gyro = lsqnonlin(@(theta_pr_gyro) gyroCostFunctLSQNONLIN(theta_pr_gyro, QS_time_interval_calib_info_matrix, omega_x, omega_y, omega_z, 1/freq), theta_pr_gyro, [], [], option); 295 | 296 | misal_matrix = [1, theta_pr_gyro(2), theta_pr_gyro(3); theta_pr_gyro(4), 1, theta_pr_gyro(6); theta_pr_gyro(7), theta_pr_gyro(8), 1]; 297 | scale_matrix = [theta_pr_gyro(1), 0, 0; 0, theta_pr_gyro(5), 0; 0, 0, theta_pr_gyro(9)]; 298 | 299 | disp(scale_matrix); % gyro 300 | disp(misal_matrix); % gyro 301 | 302 | disp(['Accelerometer''','s Scaling Matrix taken from datasheet:']); 303 | disp(DS_a_scale); 304 | disp(['Accelerometer''','s Misalignment Matrix taken from datasheet:']); 305 | format bank 306 | disp(DS_a_misal); 307 | disp(['Gyroscope''','s Scaling Matrix taken from datasheet:']); 308 | format short 309 | disp(DS_g_scale); 310 | disp(['Gyroscope''','s Misalignment Matrix taken from datasheet:']); 311 | format bank 312 | disp(DS_g_misal); 313 | disp('-----------------------------------------------------------------') 314 | 315 | format short 316 | [comp_a_scale, comp_a_misal, comp_g_scale, comp_g_misal] = ... 317 | obtainComparableMatrix(estimated_scalingMatrix, estimated_misalignmentMatrix, scale_matrix, misal_matrix); 318 | 319 | disp(['Accelerometer''','s Estimated Scaling Matrix:']); 320 | disp(comp_a_scale); 321 | disp(['Accelerometer''','s Estimated Misalignment Matrix:']); 322 | disp(comp_a_misal); 323 | disp(['Gyroscope''','s Estimated Scaling Matrix:']); 324 | disp(comp_g_scale); 325 | disp(['Gyroscope''','s Estimated Misalignment Matrix:']); 326 | disp(comp_g_misal); -------------------------------------------------------------------------------- /srcs/compute_time_interval_info_matrix.m: -------------------------------------------------------------------------------- 1 | % input: filter, the static indicator for each sample obtained by a threshold, 2 | % freq, how many samples are in one second 3 | % output: consecutive static session recorded in QS_time_interval_info_matrix 4 | % of size Nx3, and refined s_filter with small dynamic and static session 5 | % outliers removed. 6 | function [QS_time_interval_info_matrix, filter]=compute_time_interval_info_matrix(filter, freq) 7 | l = 1; 8 | QS_time_interval_info_matrix = zeros(1, 1 + 1 + 1); % record the static periods, unfortunately, dynamically changing size 9 | samples = 0; % how many samples does this static period have 10 | start = 0; % where does this static period starts 11 | flag = 0; % what is the state of the last sample, 0 for dynamic, 1 for static 12 | 13 | if filter(1) == 0 14 | flag = 0; 15 | else 16 | flag = 1; 17 | start = 1; 18 | end 19 | % cycle to determine the QS_time_interval_info_matrix 20 | for i = 1:length(filter) 21 | if flag == 0 && filter(i) == 0 22 | 0; % do nothing 23 | elseif flag == 1 && filter(i) == 1 24 | samples = samples + 1; 25 | elseif flag == 1 && filter(i) == 0 % take action when static chnages to dynamic 26 | QS_time_interval_info_matrix(l, 1:3) = [start, i - 1, samples]; 27 | l = l + 1; 28 | flag = 0; 29 | elseif flag == 0 && filter(i) == 1 % take action when dynamic chnages to static 30 | start = i; 31 | samples = 1; 32 | flag = 1; 33 | end 34 | end 35 | % Huai { refine QS_time_interval_info_matrix by two criteria, 36 | % (1) if a dynamic period is too short, less than freq, set it as 37 | % static, (2) if a static period is too short, set it as dynamic 38 | % we prefer to set dynamic to static if (1) is met 39 | 40 | % we do it several times, as some spikes may merge into another spike 41 | % meeting the two criteria. There are much better ways to handle this 42 | % problem than using loop, but for time sake, I don't do that. 43 | 44 | moreIter=true; % do we need more iterations to remove duds 45 | maxIter=10; % do not run too many times 46 | howMany=0; 47 | while(moreIter&& howMany0&&QS_time_interval_info_matrix(j,3) 8 | 9 | 0.090827738978113 10 | 0.0600149039251388 11 | 0.834788648002395 12 | 13 | 0.00964929436000702 0 0 14 | 0 0.00971560433303835 0 15 | 0 0 0.00950177420049073 16 | 17 | 1 0.0392840650199062 -0.0030011326791253 18 | 0 1 0.00574180807477407 19 | 0 0 1 20 | 21 | 0.042991269971998 3.18876626585406 3.93147751605996 22 | 23 | 24 | Local minimum possible. 25 | 26 | lsqnonlin stopped because the final change in the sum of squares relative to 27 | its initial value is less than the selected value of the function tolerance. 28 | 29 | 30 | 31 | 0.0169168571737101 0 0 32 | 0 0.0172774929349165 0 33 | 0 0 0.0178150273107602 34 | 35 | 1 0.0363319949565974 0.00388159283959375 36 | -0.0138774985314868 1 0.00624283960012139 37 | 0.0115541959648562 -0.00839184699522386 1 38 | 39 | Accelerometer's Scaling Matrix taken from datasheet: 40 | 102.040816326531 0 0 41 | 0 102.040816326531 0 42 | 0 0 102.040816326531 43 | 44 | Accelerometer's Misalignment Matrix taken from datasheet: 45 | 1.00 0 0 46 | 0 1.00 0 47 | 0 0 1.00 48 | 49 | Gyroscope's Scaling Matrix taken from datasheet: 50 | 57.2958 0 0 51 | 0 57.2958 0 52 | 0 0 57.2958 53 | 54 | Gyroscope's Misalignment Matrix taken from datasheet: 55 | 1.00 0 0 56 | 0 1.00 0 57 | 0 0 1.00 58 | 59 | ----------------------------------------------------------------- 60 | Accelerometer's Estimated Scaling Matrix: 61 | 103.6345 0 0 62 | 0 102.9272 0 63 | 0 0 105.2435 64 | 65 | Accelerometer's Estimated Misalignment Matrix: 66 | 0.9994 -0.0492 -0.0164 67 | 0.0097 0.9998 -0.0157 68 | 0.0201 0.0098 0.9998 69 | 70 | Gyroscope's Estimated Scaling Matrix: 71 | 59.1126 0 0 72 | 0 57.8788 0 73 | 0 0 56.1324 74 | 75 | Gyroscope's Estimated Misalignment Matrix: 76 | 0.9989 -0.0464 -0.0233 77 | 0.0236 0.9991 -0.0166 78 | 0.0088 0.0187 0.9999 -------------------------------------------------------------------------------- /srcs/obtainComparableMatrix.m: -------------------------------------------------------------------------------- 1 | function [ comp_a_scale_matrix, comp_a_misal_matrix, ... 2 | comp_g_scale_matrix, comp_g_misal_matrix ] = obtainComparableMatrix(... 3 | acc_scale_matrix, acc_misal_matrix, gyro_scale_matrix, ... 4 | gyro_misal_matrix ) 5 | 6 | % acc misal parameter taken from datasheet 7 | alpha_xz_6 = 0.01; 8 | alpha_xy_6 = -0.02; 9 | alpha_yx_6 = 0.01; 10 | 11 | theta = - alpha_xz_6; 12 | w = [0 0 1]; 13 | 14 | q = [cos(theta/2) sin(theta/2)*w]; 15 | 16 | r_11 = q(1)^2 + q(2)^2 - q(3)^2 - q(4)^2; 17 | r_12 = 2*(q(2)*q(3) - q(1)*q(4)); 18 | r_13 = 2*(q(2)*q(4) + q(1)*q(3)); 19 | r_21 = 2*(q(2)*q(3) + q(1)*q(4)); 20 | r_22 = q(1)^2 - q(2)^2 + q(3)^2 - q(4)^2; 21 | r_23 = 2*(q(3)*q(4) - q(1)*q(2)); 22 | r_31 = 2*(q(2)*q(4) - q(1)*q(3)); 23 | r_32 = 2*(q(3)*q(4) + q(1)*q(2)); 24 | r_33 = q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2; 25 | 26 | R_xz = [r_11, r_12, r_13; r_21, r_22, r_23; r_31, r_32, r_33]; 27 | 28 | theta = - alpha_xy_6; 29 | w = [0 1 0]; 30 | 31 | q = [cos(theta/2) sin(theta/2)*w]; 32 | 33 | r_11 = q(1)^2 + q(2)^2 - q(3)^2 - q(4)^2; 34 | r_12 = 2*(q(2)*q(3) - q(1)*q(4)); 35 | r_13 = 2*(q(2)*q(4) + q(1)*q(3)); 36 | r_21 = 2*(q(2)*q(3) + q(1)*q(4)); 37 | r_22 = q(1)^2 - q(2)^2 + q(3)^2 - q(4)^2; 38 | r_23 = 2*(q(3)*q(4) - q(1)*q(2)); 39 | r_31 = 2*(q(2)*q(4) - q(1)*q(3)); 40 | r_32 = 2*(q(3)*q(4) + q(1)*q(2)); 41 | r_33 = q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2; 42 | 43 | R_xy = [r_11, r_12, r_13; r_21, r_22, r_23; r_31, r_32, r_33]; 44 | 45 | theta = - alpha_yx_6; 46 | w = [1 0 0]; 47 | 48 | q = [cos(theta/2) sin(theta/2)*w]; 49 | 50 | r_11 = q(1)^2 + q(2)^2 - q(3)^2 - q(4)^2; 51 | r_12 = 2*(q(2)*q(3) - q(1)*q(4)); 52 | r_13 = 2*(q(2)*q(4) + q(1)*q(3)); 53 | r_21 = 2*(q(2)*q(3) + q(1)*q(4)); 54 | r_22 = q(1)^2 - q(2)^2 + q(3)^2 - q(4)^2; 55 | r_23 = 2*(q(3)*q(4) - q(1)*q(2)); 56 | r_31 = 2*(q(2)*q(4) - q(1)*q(3)); 57 | r_32 = 2*(q(3)*q(4) + q(1)*q(2)); 58 | r_33 = q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2; 59 | 60 | R_yx = [r_11, r_12, r_13; r_21, r_22, r_23; r_31, r_32, r_33]; 61 | 62 | comp_a_scale_matrix = inv(acc_scale_matrix); 63 | comp_a_misal_matrix = inv(R_xz*R_xy*R_yx*acc_misal_matrix); 64 | comp_g_scale_matrix = inv(gyro_scale_matrix); 65 | comp_g_misal_matrix = inv(R_xz*R_xy*R_yx*gyro_misal_matrix); 66 | 67 | 68 | 69 | end 70 | 71 | -------------------------------------------------------------------------------- /srcs/rotationRK4.m: -------------------------------------------------------------------------------- 1 | % Huai { the input dt, namely, sampling interval, is essential for quaternion integration. To be 2 | % accurate, it is advisable to put timestamp for every observation. But for 3 | % MEMS IMU, their timestamp really have a consecutive difference of dt as 4 | % their clcok is not so nice. 5 | function [ R ] = rotationRK4( omega, dt) 6 | 7 | omega_x = omega(1,:); 8 | omega_y = omega(2,:); 9 | omega_z = omega(3,:); 10 | 11 | num_samples = length(omega_x); 12 | 13 | q_k = fromOmegaToQ([omega_x(1); omega_y(1); omega_z(1)], [dt])'; 14 | q_next_k = q_k; % was [0; 0; 0; 0]; changed by Huai 15 | 16 | for i = 1:num_samples - 1 17 | 18 | % first Runge-Kutta coefficient 19 | q_i_1 = q_k; 20 | OMEGA_omega_t_k = ... 21 | [0 -omega_x(i) -omega_y(i) -omega_z(i); 22 | omega_x(i) 0 omega_z(i) -omega_y(i); 23 | omega_y(i) -omega_z(i) 0 omega_x(i); 24 | omega_z(i) omega_y(i) -omega_x(i) 0 ]; 25 | k_1 = (1/2)*OMEGA_omega_t_k*q_i_1; 26 | 27 | % second Runge-Kutta coefficient 28 | q_i_2 = q_k + dt*(1/2)*k_1; 29 | OMEGA_omega_t_k_plus_half_dt = ... 30 | [0 -(omega_x(i) + omega_x(i + 1))/2 -(omega_y(i) + omega_y(i + 1))/2 -(omega_z(i) + omega_z(i + 1))/2; 31 | (omega_x(i) + omega_x(i + 1))/2 0 (omega_z(i) + omega_z(i + 1))/2 -(omega_y(i) + omega_y(i + 1))/2; 32 | (omega_y(i) + omega_y(i + 1))/2 -(omega_z(i) + omega_z(i + 1))/2 0 (omega_x(i) + omega_x(i + 1))/2; 33 | (omega_z(i) + omega_z(i + 1))/2 (omega_y(i) + omega_y(i + 1))/2 -(omega_x(i) + omega_x(i + 1))/2 0 ]; 34 | k_2 = (1/2)*OMEGA_omega_t_k_plus_half_dt*q_i_2; 35 | 36 | % third Runge-Kutta coefficient 37 | q_i_3 = q_k + dt*(1/2)*k_2; 38 | OMEGA_omega_t_k_plus_half_dt = ... 39 | [0 -(omega_x(i) + omega_x(i + 1))/2 -(omega_y(i) + omega_y(i + 1))/2 -(omega_z(i) + omega_z(i + 1))/2; 40 | (omega_x(i) + omega_x(i + 1))/2 0 (omega_z(i) + omega_z(i + 1))/2 -(omega_y(i) + omega_y(i + 1))/2; 41 | (omega_y(i) + omega_y(i + 1))/2 -(omega_z(i) + omega_z(i + 1))/2 0 (omega_x(i) + omega_x(i + 1))/2; 42 | (omega_z(i) + omega_z(i + 1))/2 (omega_y(i) + omega_y(i + 1))/2 -(omega_x(i) + omega_x(i + 1))/2 0 ]; 43 | k_3 = (1/2)*OMEGA_omega_t_k_plus_half_dt*q_i_3; 44 | 45 | % forth Runge-Kutta coefficient 46 | q_i_4 = q_k + dt*1*k_3; 47 | OMEGA_omega_t_k_plus_dt = ... 48 | [0 -omega_x(i + 1) -omega_y(i + 1) -omega_z(i + 1); 49 | omega_x(i + 1) 0 omega_z(i + 1) -omega_y(i + 1); 50 | omega_y(i + 1) -omega_z(i + 1) 0 omega_x(i + 1); 51 | omega_z(i + 1) omega_y(i + 1) -omega_x(i + 1) 0 ]; 52 | k_4 = (1/2)*OMEGA_omega_t_k_plus_dt*q_i_4; 53 | 54 | q_next_k = q_k + dt*((1/6)*k_1 + (1/3)*k_2 + (1/3)*k_3 + (1/6)*k_4); 55 | 56 | q_next_k = q_next_k/norm(q_next_k); 57 | 58 | q_k = q_next_k; 59 | 60 | end 61 | 62 | R = inv(fromQtoR(q_next_k)); 63 | 64 | end 65 | 66 | --------------------------------------------------------------------------------