Оцените ориентацию и высоту Используя IMU, магнитометр и высотомер

Этот пример показывает, как плавить данные из акселерометра с 3 осями, гироскопа с 3 осями, магнитометр с 3 осями (вместе обычно называемый датчиком MARG для Магнитного, Углового Уровня и Силой тяжести), и высотомер с 1 осью, чтобы оценить ориентацию и высоту.

Setup симуляции

Эта симуляция обрабатывает данные о датчике на нескольких уровнях. IMU (акселерометр и гироскоп) обычно запускается на самом высоком уровне. Магнитометр обычно запускается на более низком уровне, чем IMU и выполнениях высотомера на самом низком уровне. Изменение частот дискретизации заставляет части алгоритма сплава запускаться более часто и может влиять на производительность.

% Set the sampling rate for IMU sensors, magnetometer, and altimeter.
imuFs = 100;
altFs = 10;
magFs = 25;
imuSamplesPerAlt = fix(imuFs/altFs);
imuSamplesPerMag = fix(imuFs/magFs);

% Set the number of samples to simulate.
N = 1000;

% Construct object for other helper functions.
hfunc = Helper10AxisFusion;

Задайте траекторию

Корпус датчика вращается обо всех трех осях при колебании в положении вертикально. Увеличение колебаний значения как симуляция продолжается.

% Define the initial state of the sensor body
initPos = [0, 0, 0];       % initial position (m)
initVel = [0, 0, -1];      % initial linear velocity (m/s)
initOrient = ones(1, 'quaternion');

% Define the constant angular velocity for rotating the sensor body
% (rad/s).
angVel = [0.34 0.2 0.045];

% Define the acceleration required for simple oscillating motion of the
% sensor body.
fc = 0.2;
t = 0:1/imuFs:(N-1)/imuFs;
a = 1;
oscMotionAcc = sin(2*pi*fc*t);
oscMotionAcc = hfunc.growAmplitude(oscMotionAcc);

% Construct the trajectory object
traj = kinematicTrajectory('SampleRate', imuFs, ...
    'Velocity', initVel, ...
    'Position', initPos, ...
    'Orientation', initOrient);

Настройка датчика

Акселерометр, гироскоп и магнитометр моделируются с помощью imuSensor. Высотомер моделируется с помощью altimeterSensor. Значения, используемые в настройках датчика, соответствуют действительным значениям датчика MEMS.

imu = imuSensor('accel-gyro-mag', 'SampleRate', imuFs);

% Accelerometer
imu.Accelerometer.MeasurementRange =  19.6133;
imu.Accelerometer.Resolution = 0.0023928;
imu.Accelerometer.ConstantBias = 0.19;
imu.Accelerometer.NoiseDensity = 0.0012356;

% Gyroscope
imu.Gyroscope.MeasurementRange = deg2rad(250);
imu.Gyroscope.Resolution = deg2rad(0.0625);
imu.Gyroscope.ConstantBias = deg2rad(3.125);
imu.Gyroscope.AxesMisalignment = 1.5;
imu.Gyroscope.NoiseDensity = deg2rad(0.025);

% Magnetometer
imu.Magnetometer.MeasurementRange = 1000;
imu.Magnetometer.Resolution = 0.1;
imu.Magnetometer.ConstantBias = 100;
imu.Magnetometer.NoiseDensity = 0.3/sqrt(50);

% altimeter
altimeter = altimeterSensor('UpdateRate', altFs, 'NoiseDensity', 2*0.1549);

Фильтр Fusion

Создайте ahrs10filter и сконфигурируйте.

fusionfilt = ahrs10filter;
fusionfilt.IMUSampleRate = imuFs;

Установите начальные значения для фильтра сплава.

initstate = zeros(18,1);
initstate(1:4) = compact(initOrient);
initstate(5) = initPos(3);
initstate(6) = initVel(3);
initstate(7:9) = imu.Gyroscope.ConstantBias/imuFs;
initstate(10:12) = imu.Accelerometer.ConstantBias/imuFs;
initstate(13:15) = imu.MagneticField;
initstate(16:18) = imu.Magnetometer.ConstantBias;
fusionfilt.State = initstate;

Инициализируйте ковариационную матрицу состояния фильтра сплава. Наземная истина используется для начальных состояний, таким образом, должно быть мало ошибки в оценках.

icv = diag([1e-8*[1 1 1 1 1 1 1], 1e-3*ones(1,11)]);
fusionfilt.StateCovariance = icv;

Магнитометр и шумы измерения высотомера являются шумами наблюдения, сопоставленными с датчиками, используемыми внутренним Фильтром Калмана в ahrs10filter. Эти значения обычно прибывали бы из таблицы данных датчика.

magNoise =  2*(imu.Magnetometer.NoiseDensity(1).^2)*imuFs;
altimeterNoise = 2*(altimeter.NoiseDensity).^2 * altFs;

Шумы процесса фильтра используются, чтобы настроить фильтр на желаемую производительность.

fusionfilt.AccelerometerNoise = [1e-1 1e-1 1e-4];
fusionfilt.AccelerometerBiasNoise = 1e-8;
fusionfilt.GeomagneticVectorNoise = 1e-12;
fusionfilt.MagnetometerBiasNoise = 1e-12;
fusionfilt.GyroscopeNoise = 1e-12;

Опция дополнительной симуляции: средство просмотра

По умолчанию эта симуляция строит ошибки оценки в конце симуляции. Чтобы просмотреть и предполагаемое положение и ориентацию наряду с наземной истиной как, симуляция запускается, установите переменную usePoseViewer на true.

usePoseViewer = false;

Цикл симуляции

q = initOrient;
firstTime = true;

actQ = zeros(N,1, 'quaternion');
expQ = zeros(N,1, 'quaternion');
actP = zeros(N,1);
expP = zeros(N,1);

for ii = 1: N
    % Generate a new set of samples from the trajectory generator
    accBody = rotateframe(q, [0 0 +oscMotionAcc(ii)]);
    omgBody = rotateframe(q, angVel);
    [pos, q, vel, acc] = traj(accBody, omgBody);

    % Feed the current position and orientation to the imuSensor object
    [accel, gyro, mag] = imu(acc, omgBody, q);
    fusionfilt.predict(accel, gyro);

    % Fuse magnetometer samples at the magnetometer sample rate
    if ~mod(ii,imuSamplesPerMag)
        fusemag(fusionfilt, mag, magNoise);
    end

    % Sample and fuse the altimeter output at the altimeter sample rate
    if ~mod(ii,imuSamplesPerAlt)
        altHeight = altimeter(pos);

        % Use the |fusealtimeter| method to update the fusion filter with
        % the altimeter output.
        fusealtimeter(fusionfilt,altHeight,altimeterNoise);
    end

    % Log the actual orientation and position
    [actP(ii), actQ(ii)] = pose(fusionfilt);

    % Log the expected orientation and position
    expQ(ii) = q;
    expP(ii) = pos(3);

    if usePoseViewer
        hfunc.view(actP(ii), actQ(ii),expP(ii), expQ(ii)); %#ok<*UNRCH>
    end

end

Постройте производительность фильтра

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

hfunc.plotErrs(actP, actQ, expP, expQ);

Заключение

Этот пример показывает как ahrs10filter, чтобы выполнить сплав датчика с 10 осями для высоты и ориентацию.

Для просмотра документации необходимо авторизоваться на сайте