Exponenta Banner
exponenta event banner

Удаление смещения из измерения угловой скорости

Открыть сценарий в реальном времени

В этом примере показано, как удалить смещение гироскопа из IMU с помощью imufilter.

Использовать kinematicTrajectory для создания траектории с двумя деталями. Первая часть имеет постоянную угловую скорость вокруг осей y и z. Вторая часть имеет изменяющуюся угловую скорость во всех трех осях.

duration = 60*8;
fs = 20;
numSamples = duration * fs;
rng('default') % Seed the RNG to reproduce noisy sensor measurements.

initialAngVel = [0,0.5,0.25];
finalAngVel = [-0.2,0.6,0.5];
constantAngVel = repmat(initialAngVel,floor(numSamples/2),1);
varyingAngVel = [linspace(initialAngVel(1), finalAngVel(1), ceil(numSamples/2)).', ...
    linspace(initialAngVel(2), finalAngVel(2), ceil(numSamples/2)).', ...
    linspace(initialAngVel(3), finalAngVel(3), ceil(numSamples/2)).'];

angVelBody = [constantAngVel; varyingAngVel];
accBody = zeros(numSamples,3);

traj = kinematicTrajectory('SampleRate',fs);

[~,qNED,~,accNED,angVelNED] = traj(accBody,angVelBody);

Создание imuSensor object™ системы, IMU, с неидеальным гироскопом. Звонить IMU с ускорением «земля-истина», угловой скоростью и ориентацией.

IMU = imuSensor('accel-gyro', ...
    'Gyroscope',gyroparams('RandomWalk',0.003,'ConstantBias',0.3), ...
    'SampleRate',fs);

[accelReadings, gyroReadingsBody] = IMU(accNED,angVelNED,qNED);

Создание imufilter Системный объект, fuse. Звонить fuse с моделируемыми показаниями акселерометра и гироскопа.

fuse = imufilter('SampleRate',fs, 'GyroscopeDriftNoise', 1e-6);

[~,angVelBodyRecovered] = fuse(accelReadings,gyroReadingsBody);

Постройте график угловой скорости «земля-истина», показаний гироскопа и восстановленной угловой скорости для каждой оси.

Угловая скорость, возвращенная из imufilter компенсирует эффект смещения гироскопа во времени и сходится к истинной угловой скорости.

time = (0:numSamples-1)'/fs;

figure(1)
plot(time,angVelBody(:,1), ...
     time,gyroReadingsBody(:,1), ...
     time,angVelBodyRecovered(:,1))
title('X-axis')
legend('True Angular Velocity', ...
       'Gyroscope Readings', ...
       'Recovered Angular Velocity')
ylabel('Angular Velocity (rad/s)')

Figure contains an axes. The axes with title X-axis contains 3 objects of type line. These objects represent True Angular Velocity, Gyroscope Readings, Recovered Angular Velocity.

figure(2)
plot(time,angVelBody(:,2), ...
     time,gyroReadingsBody(:,2), ...
     time,angVelBodyRecovered(:,2))
title('Y-axis')
ylabel('Angular Velocity (rad/s)')

Figure contains an axes. The axes with title Y-axis contains 3 objects of type line.

figure(3)
plot(time,angVelBody(:,3), ...
     time,gyroReadingsBody(:,3), ...
     time,angVelBodyRecovered(:,3))
title('Z-axis')
ylabel('Angular Velocity (rad/s)')
xlabel('Time (s)')

Figure contains an axes. The axes with title Z-axis contains 3 objects of type line.

Документация по комплекту инструментов для слияния и отслеживания датчиков

Поддержка