Converting sensor frames using Aerospace Toolbox

Question

Jason LaPenta 2020-3-27

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/513360-converting-sensor-frames-using-aerospace-toolbox

回答： James Tursa 2020-4-6

Hello,

I'm having trouble wrapping my head around writing quarternion transforms in Matlab to convert between body frames and navigation frames. Does anyone know of some Matlab tutorials, videos, or example code that might help me? I've already looked at the Matlab Help "About Aerospace Coordinate Systems", "Coordinate Systems", "Orientation, Position, and Coordinate Systems", and other provided by Matlab Help, but they are insuffient for me. Specifically, I'm trying to use the Aerospace blockset and toolbox with the Sensor Fusion and Tracking Toolbox, and convert between body-fixed, inertial frame, and navigation frames of the 6DOF (Quaternion) block and imuSensor. Currently I'm doing the following, which I figured out by trial and error.

function [acc, gyro, mag] = fcn(acc_be, angVel, ori)
% Notes
% the input is from the 6DOF (Quaternion) block
% the output is from the imuSensor Model
accBody = [0 0 0]';
gyroBody = [0 0 0]';
magBody = [0 0 0]';
persistent IMU
if isempty(IMU)
    
    IMU = imuSensor('accel-gyro-mag');
    IMU.SampleRate = 200;
end
ori_Q = quaternion(ori');
quatRotator = quaternion([0  ,180,180],'eulerd','ZYX','frame');
[aBody, gBody, mBody] = IMU(acc',angVel',ori_Q);
    
aBody = rotateframe(quatRotator,aBody);
aBody(3) = -aBody(3);
accBody = aBody';
gyroBody = gBody';
magBody = mBody';
end