M5Stackのジャイロセンサの角速度からの姿勢推定

以下の記事を参考に。
watako-lab.com
hamachannel.hatenablog.com

上の２つのページには同じ式が異なる表記で載っている。

この式の意味は、
（X-Y-Zオイラー角（＝ロール・ピッチ・ヨー）の時間微分） =　（姿勢オイラー角から計算される変換行列）　x 　（ジャイロセンサ座標系での角速度）

つまり、現時点でのオイラー角と角速度がわかっていれば、オイラー角の時間微分が計算できる。
オイラー角の時間微分がわかれば、次の時点でのオイラー角が計算できる。よって姿勢推定ができる。

例によってMatlabのMQTTサーバ側コード

clear; close all;

global h; global dir;
global r; global p; global y;
global t; global ts; global rs; global ys; global ps;
global axs; global ays; global azs; 
global wxs; global wys; global wzs;
global ax_bias; global ay_bias; global az_bias;
global wx_bias; global wy_bias; global wz_bias;
global g;

g = 9.81; % gravity constant

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

pt = [0 0 0];
dir = [1 0 0 1];
ts=[];
rs = []; ps = []; ys = [];
axs=[]; ays=[];azs=[];
wxs=[];wys=[];wzs=[];
r = 0; p = 0; y = 0;


ax_bias = -0.0096;
ay_bias = 0.0028;
az_bias = 1.065-1.0;
wx_bias = 11.5033;
wy_bias = -14.633;
wz_bias = -6.6056;

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

global maxn; global avet;
maxn = 20; avet=0.1;

コールバック関数
computeGyro.m

function computeGyro(~,json)
  global t;global y; global p; global r;
  global ts; global ys; global ps; global rs;
  global wxs; global wys; global wzs;
  global h; global dir;
  global maxn; global avet;
  global g;
  global ax_bias; global ay_bias; global az_bias; 
  global wx_bias; global wy_bias; global wz_bias; 

  json = strrep(json,"'",'"');
  json =eraseBetween(json,1,1);
  json = eraseBetween(json,strlength(json),strlength(json));
  data = jsondecode(json); 
  wx = (data.wx-wx_bias)*pi/180;
  wy = (data.wy-wy_bias)*pi/180;
  wz = (data.wz-wz_bias)*pi/180;
  tnext = data.t;
  tnext = tnext/1000;
  dt = tnext-t;
  t = tnext;
  ts = horzcat(ts, t);
  R = [1 sin(r)*sin(p)/cos(p) cos(r)*sin(p)/cos(p);
       0 cos(r) -sin(r);
       0 sin(r)/cos(p) cos(r)/cos(p)];
  drpy = R*[wx wy wz]';
  dr = drpy(1);
  dp = drpy(2);
  dy = drpy(3);
  r = r + dr*dt;
  p = p + dp*dt;
  y = y + dy*dt;
  rs = horzcat(rs, r); 
  ps = horzcat(ps, p); 
  ys = horzcat(ys, y);
  wxs = horzcat(wxs, wx); 
  wys = horzcat(wys, wy); 
  wzs = horzcat(wzs, wz);
  
  subplot(2,2,[1,3]);
  xfm = makehgtform('xrotate',r,'yrotate',p,'zrotate',y);
  newdir = xfm * dir';
  h.UData = newdir(1);
  h.VData = newdir(2);
  h.WData = newdir(3);
  
  if(length(ts)>maxn)
    ts = ts(2:end);
    rs = rs(2:end);
    ps = ps(2:end);
    ys = ys(2:end);
    wxs = wxs(2:end);
    wys = wys(2:end);
    wzs = wzs(2:end);    
  end

  subplot(2,2,2);
  gca;
  plot(ts,rs,'r');
  hold on;
  plot(ts,ps,'b');
  plot(ts,ys,'g');
  xlim([ts(1),ts(end)]);
  legend('Roll','Pitch','Yaw');
  
  subplot(2,2,4);
  gca;
  plot(ts,wxs,'c');
  hold on;
  plot(ts,wys,'m');
  plot(ts,wzs,'k');   
  xlim([ts(1),ts(end)]);
  legend('ωx','ωy','ωz');
  drawnow;
  
end

www.youtube.com

ロールピッチヨーの角度の単位はradで計算されるはずなので、値がおかしい気がする。ノイズがのっているようにも見えるので、次はフィルタを試してみる。