ecompass

Ориентация от показаний магнитометра и акселерометра

Синтаксис

orientation = ecompass(accelerometerReading,magnetometerReading)

orientation = ecompass(accelerometerReading,magnetometerReading,orientationFormat)

Описание

orientation = ecompass(accelerometerReading,magnetometerReading) возвращает quaternion, который может вращать количества от родительского элемента (NED) кадр дочернему элементу (датчик) кадр.

пример

orientation = ecompass(accelerometerReading,magnetometerReading,orientationFormat) задает формат ориентации как матрицу вращения или quaternion.

Примеры

свернуть все

Определите наклон Бостона

Открыть скрипт

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

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

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')

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])

quaterionEulerAngles =

   14.6563         0         0


rotmatEulerAngles =

   14.6563         0         0

Определите вектор силы тяжести

Скрипт Open Live Script

Используйте 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

Отследите вращающуюся платформу

Скрипт Open Live Script

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

Сгенерируйте траекторию наземной истины

Опишите ориентацию наземной истины платформы в зависимости от времени. Используйте Систему kinematicTrajectory object™, чтобы создать траекторию для платформы, которая не имеет никакого перевода и вращений о его оси z.

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, чтобы подражать данным, полученным от IMU, который содержит идеальный магнитометр и идеальный акселерометр.

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

Плавьте данные IMU, чтобы оценить ориентацию

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

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

Измените параметры Системного объекта IMU, чтобы аппроксимировать реалистические данные о датчике IMU. Сбросьте IMU и затем вызовите его с тем же ускорением наземной истины, угловой скоростью и ориентацией. Используйте ecompass, чтобы плавить данные 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')

Входные параметры

свернуть все

`accelerometerReading` — Показания акселерометра в системе координат корпуса датчика ^(m/s2)
N-by-3 матрица

Показания акселерометра в системе координат корпуса датчика в ^m/s2, заданном как N-by-3 матрица. Столбцы матрицы соответствуют x - y - и z - оси корпуса датчика. Строки в матрице, N, соответствуют отдельным выборкам. Показания акселерометра нормированы перед использованием в функции.

Типы данных: single | double

`magnetometerReading` — Показания магнитометра в системе координат корпуса датчика (µT)
N-by-3 матрица

Показания магнитометра в системе координат корпуса датчика в µT, заданном как N-by-3 матрица. Столбцы матрицы соответствуют x - y - и z - оси корпуса датчика. Строки в матрице, N, соответствуют отдельным выборкам. Показания магнитометра нормированы перед использованием в функции.

Типы данных: single | double

`orientationFormat` — Формат раньше описывал ориентацию
`'quaternion'` (значение по умолчанию) | `'rotmat'`

Формат раньше описывал ориентацию, заданную как 'quaternion' или 'rotmat'.

Типы данных: char | string

Выходные аргументы

свернуть все

`orientation` — Ориентация, которая вращает количества от глобальной системы координат до системы координат корпуса датчика
N-by-1 вектор кватернионов (значение по умолчанию) | 3 3 N массивом

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

'quaternion' – N-by-1 вектор кватернионов с тем же базовым типом данных как вход
'rotmat' – 3 3 N массивом совпадающий тип данных как вход

Типы данных: quaternion | single | double

Алгоритмы

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

Матрица вращения сначала вычисляется с посредником:

$R = [\begin{matrix} (a \times m) \times a & a \times m & a \end{matrix}]$

и затем нормированный по столбцам. a и m являются входом accelerometerReading и входом magnetometerReading, соответственно.

Чтобы понять матричное вычисление вращения, рассмотрите произвольную точку на Земле и ее соответствующем локальном кадре NED. Примите каркас кузова датчика, [x, y, z], с тем же источником.

Вспомните, что ориентация корпуса датчика задана как оператор вращения (матрица вращения или кватернион) требуемый вращать количество от родительского элемента (NED) кадр дочернему элементу (корпус датчика) кадр:

$[\begin{matrix} R \end{matrix}] [\begin{matrix} p_{родительский элемент} \end{matrix}] = [\begin{matrix} p_{дочерний элемент} \end{matrix}]$

где

R является 3х3 матрицей вращения, которая может быть интерпретирована как ориентация дочернего кадра.
_{Родительский элемент} p является вектором 3 на 1 в родительском кадре.
_{Дочерний элемент} p является вектором 3 на 1 в дочернем кадре.

Для стабильного корпуса датчика акселерометр возвращает ускорение из-за силы тяжести. Если корпус датчика отлично выравнивается с системой координат NED, все ускорение из-за силы тяжести приезжает z - ось и чтения акселерометра [0 0 1]. Считайте матрицу вращения требуемой вращать количество от системы координат NED до количества, обозначенного акселерометром.

$[\begin{matrix} r_{11} & r_{21} & r_{31} \\ r_{12} & r_{22} & r_{32} \\ r_{13} & r_{23} & r_{33} \end{matrix}] [\begin{matrix} 001 \end{matrix}] = [\begin{matrix} a_{1} \\ a_{2} \\ a_{3} \end{matrix}]$

Третий столбец матрицы вращения соответствует чтению акселерометра:

$[\begin{matrix} r_{31} \\ r_{32} \\ r_{33} \end{matrix}] = [\begin{matrix} a_{1} \\ a_{2} \\ a_{3} \end{matrix}]$

Магнитометр, читая точки к магнитному северу и находится в N-D плоскость. Снова, считайте каркас кузова датчика выровненным с системой координат NED.

По определению E - ось перпендикулярна N-D плоскость, поэтому N ⨯ D = E, в рамках некоторого амплитудного масштабирования. Если каркас кузова датчика выравнивается с NED, и ускоряющий вектор от акселерометра и вектор магнитного поля от магнитометра лежат в N-D плоскость. Поэтому m ⨯ a = y, снова с некоторым амплитудным масштабированием.

Считайте матрицу вращения требуемой вращать NED к дочернему кадру, [x y z].

$[\begin{matrix} r_{11} & r_{21} & r_{31} \\ r_{12} & r_{22} & r_{32} \\ r_{13} & r_{23} & r_{33} \end{matrix}] [\begin{matrix} 010 \end{matrix}] = [\begin{matrix} a_{1} \\ a_{2} \\ a_{3} \end{matrix}] \times [\begin{matrix} m_{1} \\ m_{2} \\ m_{3} \end{matrix}]$

Второй столбец матрицы вращения соответствует векторному произведению чтения акселерометра и чтения магнитометра:

$[\begin{matrix} r_{21} \\ r_{22} \\ r_{23} \end{matrix}] = [\begin{matrix} a_{1} \\ a_{2} \\ a_{3} \end{matrix}] \times [\begin{matrix} m_{1} \\ m_{2} \\ m_{3} \end{matrix}]$

По определению матрицы вращения, столбец 1 является векторным произведением столбцов 2 и 3:

$\begin{array}{l} [\begin{matrix} r_{11} \\ r_{12} \\ r_{13} \end{matrix}] = [\begin{matrix} r_{21} \\ r_{22} \\ r_{23} \end{matrix}] \times [\begin{matrix} r_{31} \\ r_{32} \\ r_{33} \end{matrix}] \\ = (a \times m) \times a \end{array}$

Наконец, матрица вращения нормирована по столбцам:

$R_{i j} = \frac{R_{i j}}{\sqrt{\sum_{i = 1}^{3} R_{i j}^{2}}}, \forall j$

Примечание

Алгоритм ecompass использует магнитный север, не истинный север, для системы координат NED.

Ссылки

[1] Fusion датчика с открытым исходным кодом. https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs

Расширенные возможности

Генерация кода C/C++
Генерация кода C и C++ с помощью MATLAB® Coder™.

Темы

Определите ориентацию Используя инерционные датчики

Документация

ecompass

Синтаксис

Описание

Примеры

Определите наклон Бостона

Возвратите матрицу вращения

Определите вектор силы тяжести

Отследите вращающуюся платформу

Входные параметры

`accelerometerReading` — Показания акселерометра в системе координат корпуса датчика ^(m/s2)
N-by-3 матрица

`magnetometerReading` — Показания магнитометра в системе координат корпуса датчика (µT)
N-by-3 матрица

`orientationFormat` — Формат раньше описывал ориентацию
`'quaternion'` (значение по умолчанию) | `'rotmat'`

Выходные аргументы

Алгоритмы

Примечание

Ссылки

Расширенные возможности

Генерация кода C/C++
Генерация кода C и C++ с помощью MATLAB® Coder™.

Смотрите также

Темы

Введенный в R2018b

Документация Sensor Fusion and Tracking Toolbox

Поддержка

Документация

ecompass

Синтаксис

Описание

Примеры

Определите наклон Бостона

Возвратите матрицу вращения

Определите вектор силы тяжести

Отследите вращающуюся платформу

Входные параметры

accelerometerReading — Показания акселерометра в системе координат корпуса датчика (m/s2) N-by-3 матрица

magnetometerReading — Показания магнитометра в системе координат корпуса датчика (µT) N-by-3 матрица

orientationFormat — Формат раньше описывал ориентацию 'quaternion' (значение по умолчанию) | 'rotmat'

Выходные аргументы

Алгоритмы

Примечание

Ссылки

Расширенные возможности

Генерация кода C/C++ Генерация кода C и C++ с помощью MATLAB® Coder™.

Смотрите также

Темы

Введенный в R2018b

Документация Sensor Fusion and Tracking Toolbox

Поддержка

`accelerometerReading` — Показания акселерометра в системе координат корпуса датчика ^(m/s2)
N-by-3 матрица

`magnetometerReading` — Показания магнитометра в системе координат корпуса датчика (µT)
N-by-3 матрица

`orientationFormat` — Формат раньше описывал ориентацию
`'quaternion'` (значение по умолчанию) | `'rotmat'`

Генерация кода C/C++
Генерация кода C и C++ с помощью MATLAB® Coder™.