MATLAB で C++ コードをコンパイルして MEX ファイルを作成する
20 views (last 30 days)
Show older comments
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;
0 Comments
Answers (0)
See Also
Categories
Find more on MATLAB Support Package for USB Webcams in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!