В этом примере показано, как сплавлять данные с 3-осевого акселерометра, 3-осевого гироскопа, 3-осевого магнитометра (совместно называемого датчиком MARG для магнитной, угловой скорости и силы тяжести) и 1-осевого высотомера для оценки ориентации и высоты.
Это моделирование обрабатывает данные датчиков на нескольких скоростях. 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. Значения, используемые в конфигурациях датчиков, соответствуют реальным значениям датчиков МЭМС.
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);
В этом примере показано, как ahrs10filter выполнить 10-осевое слияние датчиков по высоте и ориентации.