Удаление смещения из измерения скорости вращения
В этом примере показано, как удалить смещение гироскопа из БИНС с помощью 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(2) plot(time,angVelBody(:,2), ... time,gyroReadingsBody(:,2), ... time,angVelBodyRecovered(:,2)) title('Y-axis') ylabel('Angular Velocity (rad/s)')
figure(3) plot(time,angVelBody(:,3), ... time,gyroReadingsBody(:,3), ... time,angVelBodyRecovered(:,3)) title('Z-axis') ylabel('Angular Velocity (rad/s)') xlabel('Time (s)')
