MATLAB で C++ コードをコンパイルして MEX ファイルを作成する

Githubから、MadgwickAHRS.cppをダウンロードして、それをMATLABで用いたいです。そこで、Madgwickフィルターのパスの追加方法が分かりません。その際のコードは以下の通りです。

% Madgwickフィルターへのパスを追加

addpath('MadgwickAHRS'); % MadgwickフィルターがMATLABのパスにあることを確認

の部分が分かりません。

% 変数の初期化

dt = 0.01; %タイムステップ(100Hz)

pos1 = [0, 0, 0];

pos2 = [0, 0, 0];

vel1 = [0, 0, 0];

vel2 = [0, 0, 0];

% センサーデータの読み込み

data1 = xlsread('ho121移動.xlsx');

data2 = xlsread('ho122移動.xlsx');

% データの抽出

time = dt;

mx1 = data1(:, 2); % 磁気 x

my1 = data1(:, 3); % 磁気 y

mz1 = data1(:, 4); % 磁気 z

ax1 = data1(:, 5)*9.81% 加速度 x

ay1 = data1(:, 6)*9.81; % 加速度 y

az1 = data1(:, 7)*9.81; % 加速度 z

gx1 = data1(:, 8); % 角速度 x

gy1 = data1(:, 9); % 角速度 y

gz1 = data1(:, 10); % 角速度 z

mx2 = data2(:, 2); % 磁気 x

my2 = data2(:, 3); % 磁気 y

mz2 = data2(:, 4); % 磁気 z

ax2 = data2(:, 5)*9.81;% 加速度 x

ay2 = data2(:, 6)*9.81; % 加速度 y

az2 = data2(:, 7)*9.81; % 加速度 z

gx2 = data2(:, 8); % 角速度 x

gy2 = data2(:, 9); % 角速度 y

gz2 = data2(:, 10); % 角速度 z

% Madgwickフィルターへのパスを追加

addpath('MadgwickAHRS'); % MadgwickフィルターがMATLABのパスにあることを確認

% Madgwickフィルターの初期化

AHRS1 = MadgwickAHRS('SamplePeriod', dt, 'Beta', 0.1);

AHRS2 = MadgwickAHRS('SamplePeriod', dt, 'Beta', 0.1);

% 位置を格納する配列

positions1 = zeros(length(time), 3);

positions2 = zeros(length(time), 3);

% センサーフュージョンと位置トラッキングのループ

for i = 1:length(time)

% 向きの更新

AHRS1.Update([gx1(i), gy1(i), gz1(i)], [ax1(i), ay1(i), az1(i)], [mx1(i), my1(i), mz1(i)]);

AHRS2.Update([gx2(i), gy2(i), gz2(i)], [ax2(i), ay2(i), az2(i)], [mx2(i), my2(i), mz2(i)]);

% 向きのクォータニオンを取得

quat1 = AHRS1.Quaternion;

quat2 = AHRS2.Quaternion;

% 加速度をワールドフレームに回転

accel1_world = quatrotate(quat1, [ax1(i), ay1(i), az1(i)]);

accel2_world = quatrotate(quat2, [ax2(i), ay2(i), az2(i)]);

% 加速度を積分して速度を取得

vel1 = vel1 + accel1_world * dt;

vel2 = vel2 + accel2_world * dt;

% 速度を積分して位置を取得

pos1 = pos1 + vel1 * dt;

pos2 = pos2 + vel2 * dt;

% 位置を格納

positions1(i, :) = pos1;

positions2(i, :) = pos2;

end

% 相互距離の計算

distances = sqrt(sum((positions1 - positions2).^2, 2));

% 時間経過とともに各軸の距離をプロット

figure;

plot(time, distances);

xlabel('時間 (s)');

ylabel('距離 (m)');

title('2センサ間の各軸の距離');

grid on;