M5stackの３軸加速度センサからの姿勢推定

MQTTでのMacとM5stackとのやりとりはここの設定と同じ
seinzumtode.hatenadiary.jp

ロール角rとピッチ角pを推定する。
$r=\arctan(a_y/a_z), p=\arctan(-a_x/\sqrt{a_y^2+a_z^2})$

メインルーチン

clear; close all;

global h; global dir;

myMQTT=mqtt('tcp://127.0.0.1');
mySub = subscribe(myMQTT,'acc', 'Callback','computeAcc');

pt = [0 0 0];
dir = [1 0 0 1];
h = quiver3(pt(1),pt(2),pt(3), dir(1),dir(2),dir(3));
xlim([-1 1]);
ylim([-1 1]);
zlim([-1 1]);

コールバックハンドラ
computeAcc.m

function computeAcc(~,json)
  json = strrep(json,"'",'"')
  json =eraseBetween(json,1,1)
  json = eraseBetween(json,strlength(json),strlength(json))  
  data = jsondecode(json); 
  ax = data.ax;
  ay = data.ay;
  az = data.az;
  r = atan2(ay,az);
  p = atan2(-ax,sqrt(ay^2+az^2));
  
  global h; global dir;
  xfm = makehgtform('xrotate',r,'yrotate',p,'zrotate',0);
  newdir = xfm * dir';
  h.UData = newdir(1);
  h.VData = newdir(2);
  h.WData = newdir(3);
  drawnow;
  
end