Оцените ориентацию и высоту Используя 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 выполнять cочетание датчиков с 10 осями для высоты и ориентации.