Удалите смещение из измерения скорости вращения
В этом примере показано, как удалить смещение гироскопа из 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(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)')
