Оцените ориентацию и высоту с помощью БИНС, магнитометра и альтиметра

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

Область Setup

Эта симуляция обрабатывает данные датчика с несколькими скоростями. БИНС (акселерометр и гироскоп) обычно работает с самой высокой скоростью. Магнитометр обычно работает с меньшей скоростью, чем БИНС, а альтиметр работает с самой низкой скоростью. Изменение частот дискретизации заставляет части алгоритма слияния запускаться чаще и может повлиять на эффективность.

% 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);

Фильтр слияния

Создайте 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);

Figure Estimation Errors contains 2 axes. Axes 1 with title Orientation Error - Quaternion Distance contains an object of type line. Axes 2 with title Z Position Error contains an object of type line.

Заключение

В этом примере показано, как к ahrs10filter для выполнения 10-осевого слияния по высоте и ориентации.

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