Используйте известную силу магнитного поля, и соответствующее ускорение устройства указало истинный север в Бостоне, чтобы определить магнитный наклон Бостона.

Задайте известное ускорение и силу магнитного поля в Бостоне.

magneticFieldStrength = [19.535 -5.109 47.930];
properAcceleration = [0 0 9.8];

Передайте силу магнитного поля и ускорение к ecompass функция. ecompass функция возвращает оператор вращения кватерниона. Преобразуйте кватернион в Углы Эйлера в градусах.

q = ecompass(properAcceleration,magneticFieldStrength);
e = eulerd(q,'ZYX','frame');

Угол, e, представляет угол между истинным северным и магнитным севером в Бостоне. Условно, магнитный наклон отрицателен, когда магнитный север к западу от истинного севера. Инвертируйте угол, чтобы определить магнитный наклон.

magneticDeclinationOfBoston = -e(1)

magneticDeclinationOfBoston = -14.6563

ecompass функционируйте магнитометр предохранителей и данные об акселерометре, чтобы возвратить кватернион, который, когда используется в операторе вращения кватерниона, может вращать количества от родительского элемента (NED) система координат к дочерней системе координат. ecompass функция может также возвратить матрицы вращения, которые выполняют эквивалентные вращения как оператор кватерниона.

Задайте вращение, которое может взять родительскую систему координат, указывающую на магнитный север к дочерней системе координат, указывающей на географический север. Задайте вращение и как кватернион и как матрицу вращения. Затем преобразуйте кватернион и матрицу вращения к Углам Эйлера в градусах для сравнения.

Задайте силу магнитного поля в микротесла в Бостоне, MA, когда указано истинный север.

m = [19.535 -5.109 47.930];
a = [0 0 9.8];

Определите кватернион и матрицу вращения, которая способна к вращению системы координат с магнитного севера на истинный север. Отобразите результаты для сравнения.

q = ecompass(a,m);
quaterionEulerAngles = eulerd(q,'ZYX','frame')

quaterionEulerAngles = 1×3

   14.6563         0         0

r = ecompass(a,m,'rotmat');
theta = -asin(r(1,3));
psi = atan2(r(2,3)/cos(theta),r(3,3)/cos(theta));
rho = atan2(r(1,2)/cos(theta),r(1,1)/cos(theta));
rotmatEulerAngles = rad2deg([rho,theta,psi])

rotmatEulerAngles = 1×3

   14.6563         0         0

Используйте ecompass определить вектор силы тяжести на основе данных из вращения IMU.

Загрузите данные об инерциальном измерительном блоке (IMU).

load 'rpy_9axis.mat' sensorData Fs

Определите ориентацию корпуса датчика относительно локальной системы координат NED в зависимости от времени.

orientation = ecompass(sensorData.Acceleration,sensorData.MagneticField);

Чтобы оценить вектор силы тяжести, сначала вращайте показания акселерометра от системы координат тела датчика до системы координат NED с помощью orientation вектор кватерниона.

gravityVectors = rotatepoint(orientation,sensorData.Acceleration);

Определите вектор силы тяжести как в среднем восстановленные векторы силы тяжести в зависимости от времени.

gravityVectorEstimate = mean(gravityVectors,1)

gravityVectorEstimate = 1×3

    0.0000   -0.0000   10.2102

Плавьте смоделированный акселерометр и данные о гироскопе, чтобы отследить вращающуюся платформу с помощью и идеализированных и реалистических данных.

duration = 12;
fs = 100;
numSamples = fs*duration;

accelerationBody = zeros(numSamples,3);

angularVelocityBody = zeros(numSamples,3);
zAxisAngularVelocity = [linspace(0,4*pi,4*fs),4*pi*ones(1,4*fs),linspace(4*pi,0,4*fs)]';
angularVelocityBody(:,3) = zAxisAngularVelocity;

trajectory = kinematicTrajectory('SampleRate',fs);

[~,orientationNED,~,accelerationNED,angularVelocityNED] = trajectory(accelerationBody,angularVelocityBody);

IMU = imuSensor('accel-mag','SampleRate',fs);
[accelerometerData,magnetometerData] = IMU(accelerationNED, ...
                                           angularVelocityNED, ...
                                           orientationNED);

orientation = ecompass(accelerometerData,magnetometerData);
orientationEuler = eulerd(orientation,'ZYX','frame');

timeVector = (0:numSamples-1).'/fs;

figure(1)
plot(timeVector,orientationEuler)
legend('z-axis','y-axis','x-axis')
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation from Ideal IMU')

IMU.Accelerometer = accelparams( ...
    'MeasurementRange',20, ...
    'Resolution',0.0006, ...
    'ConstantBias',0.5, ...
    'AxesMisalignment',2, ...
    'NoiseDensity',0.004, ...
    'BiasInstability',0.5);
IMU.Magnetometer = magparams( ...
    'MeasurementRange',200, ...
    'Resolution',0.01);
reset(IMU)

[accelerometerData,magnetometerData] = IMU(accelerationNED,angularVelocityNED,orientationNED);

orientation = ecompass(accelerometerData,magnetometerData);
orientationEuler = eulerd(orientation,'ZYX','frame');

figure(2)
plot(timeVector,orientationEuler)
legend('z-axis','y-axis','x-axis')
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation from Realistic IMU')