3-осевая компенсация силы тяжести IMU

Question

У меня есть ускорение (x, y, z) и рулон, шаг, данные заголовка из 3-осевого ИДУ. Я хочу компенсировать данные ускорения на основе углов (r, p, h), измеренных гироскопом. Например, когда ИДУ является плоским и стационарным (ось z вверх), ускорения читаются [ax, ay, az] = [0,0, -1]. Когда ИДУ является плоским и неподвижным (по оси х вверх), ускорения считываются [ax, ay, az] = [- 1,0,0]. И, наконец, когда ИДУ плоский и стационарный (ось y вверх), ускорения читаются [ax, ay, az] = [0, -1,0].

Поскольку данные были собраны с IMU , а не, будучи совершенно плоским и ровным, мне нужно удалить g-компоненты из данных ускорения.

Ниже представлен мой первый пропуск при удалении g-компонентов. Другой подход, который я могу использовать, потому что я уже могу сказать, что это интенсивно вычислительно, так как размер моего файла данных увеличивается.

%% Remove gravity from X, Y, Z components 
refPlane = [r0 p0 h0]; % [2deg 4deg 60deg] IMU was not level during data collection. 

for i = 1:length(time) 
    deltaAngleX = roll(i) - refPlane(1); %degrees 
    deltaAngleY = pitch(i) - refPlane(2); %degrees 

    if (deltaAngleX > 0) % roll ++, ay --, az ++ 
     cX_X = 0; 
     cY_X = -sind (deltaAngleX) ; 
     cZ_X = cosd (deltaAngleX) ; 
    elseif (deltaAngleX < 0) 
     cX_X = 0; 
     cY_X = sind (deltaAngleX) ; 
     cZ_X = cosd (deltaAngleX) ; 
    end 

    if (deltaAngleY > 0) % roll ++, ay --, az ++ 
     cX_Y = sind (deltaAngleY) ; 
     cY_Y = 0 ; 
     cZ_Y = sind (deltaAngleY) ; 
    elseif (deltaAngleY < 0) 
     cX_Y = -sind (deltaAngleY) ; 
     cY_Y = 0 ; 
     cZ_Y = sind (deltaAngleY) ; 
    end 

    ax(i) = ax(i) + cX_X + cX_Y; 
    ay(i) = ay(i) + cY_X + cY_Y; 
    az(i) = az(i) + cZ_X + cZ_Y; 

end 

Answer 1

Вы можете использовать матрицу поворота (http://en.wikipedia.org/wiki/Rotation_matrix). В настоящее время большая часть вашего кода записывает матричное умножение, по одному компоненту за раз. Matlab быстро перемножает матрицы, поэтому он может немного ускорить работу.

Чтобы продвинуться дальше, Вы можете векторизовать код. Это большая тема, поэтому я просто даю широкую идею. Дело в том, что Matlab особенно хорошо приспособлен для обработки операций на больших матрицах и будет запускать такие операции намного быстрее, чем он будет запускать петли for.

Например, в Matlab вы можете применить функцию sin к каждому элементу матрицы M, просто написав sin(M). Используя операции, такие как repmat, попробуйте создать большую матрицу размером 2 на 2 по длине (время), которая кодирует все ваши операции вращения. Аналогичным образом создайте большой вектор размером 2 по длине (время), который кодирует все ваши векторы ускорения. Затем одно матричное умножение и операция sum в соответствии с соответствующим измерением дадут вам сразу все ваши данные.