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