Compile the C++ code in MATLAB to create a MEX file
I downloaded MadgwickAHRS.cpp from Github and want to use it in MATLAB. So, I do not know how to add the Madgwick filter path. The code for that is as follows.
% Add path to Madgwick filter
addpath(‘MadgwickAHRS’); % Make sure the Madgwick filter is in the MATLAB path
I do not understand the “%” part.
% 変数の初期化
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;I downloaded MadgwickAHRS.cpp from Github and want to use it in MATLAB. So, I do not know how to add the Madgwick filter path. The code for that is as follows.
% Add path to Madgwick filter
addpath(‘MadgwickAHRS’); % Make sure the Madgwick filter is in the MATLAB path
I do not understand the “%” part.
% 変数の初期化
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; I downloaded MadgwickAHRS.cpp from Github and want to use it in MATLAB. So, I do not know how to add the Madgwick filter path. The code for that is as follows.
% Add path to Madgwick filter
addpath(‘MadgwickAHRS’); % Make sure the Madgwick filter is in the MATLAB path
I do not understand the “%” part.
% 変数の初期化
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; matlab, madgwick, position estimation, acceleration, filter MATLAB Answers — New Questions