MATLAB实现手机重力加速度去除

做这个的来由是一哥们想要通过手机的加速度和方位角数据，分离出手机在无重力情况下的加速度（类似于NS健身环的功能）。
那怎么实现呢：首先我们有的数据是手机的三个方位角度α，β，γ以及xyz方向上的加速度a_x，a_y，a_z。这个问题的关键在于怎么找出重力加速度，并把它去掉，那我们可以先把手机的方位还原为初始状态，即α，β，γ均为0，通过使用欧拉角变换公式：
通过欧拉角变换后，重力加速度被聚集到z分量上，直接减去一个g即可剩余的加速度即是无重力情况下的加速度。以下是基于MATLAB的方法：

close all; clear all, clc
a_init = [ 0; 5; 0];% 输入加速度。
x1 = a_init(1);
y1 = a_init(2);
z1 = a_init(3);
a_update = roating(0, 0, pi/3)*a_init;% 输入倾斜角度。
% a_update = a_update + [0; 0; 9.8];
x = a_update(1);
y = a_update(2);
z = a_update(3);
scatter3(x,y,z,'ob','filled')
hold on 
scatter3(x1,y1,z1,'or','filled')
plot3([0,x],[0,y],[0,z],'-')
plot3([0,x1],[0,y1],[0,z1],'-')
grid on 
axis([-10 10 -10 10 -10 10])


function f = roating(a, b, c)
f = [cos(a)*cos(c)+sin(a)*sin(b)*sin(c) ...
    cos(c)*sin(a)*sin(b)-cos(a)*sin(c) ...
    cos(b)*sin(a);
    
    cos(b)*sin(c) ...
    cos(b)*cos(c) ...
    -sin(b);
    
    cos(a)*sin(b)*sin(c)-sin(a)*cos(c) ...
    sin(a)*sin(c)+cos(a)*cos(c)*sin(b) ...
    cos(a)*cos(b)];
end

下图是一个实例，本实例并没有去除重力加速度（%已悬起），单纯展示一下角度的变换，y轴上加速度为5，γ旋转角度60°。红色为初始点，黑色为变换后的点。