각속도 측정에서 편향 제거하기

이 예제에서는 imufilter를 사용하여 IMU로부터 자이로스코프 편향을 제거하는 방법을 보여줍니다.

kinematicTrajectory를 사용하여 2개 부분으로 구성된 궤적을 생성합니다. 첫 번째 부분은 y축과 z축에 대해 일정한 각속도를 갖습니다. 두 번째 부분은 모든 3개 축에서 변화하는 각속도를 갖습니다.

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 System object™ IMU를 만듭니다. ground-truth 가속도, 각속도, 방향을 사용하여 IMU를 호출합니다.

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

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

imufilter System object인 fuse를 만듭니다. 모델링된 가속도계 측정값과 자이로스코프 측정값을 사용하여 fuse를 호출합니다.

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

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

각 축에 대해 ground-truth 각속도, 자이로스코프 측정값, 복구된 각속도를 플로팅합니다.

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)')