角速度測定からのバイアスの削除
この例では、imufilter
を使用して IMU からジャイロスコープのバイアスを削除する方法を説明します。
kinematicTrajectory
を使用して、2 つの部分からなる軌跡を作成します。最初の部分は、y 軸と z 軸を中心とした定数値の角速度をもちます。2 番目の部分は、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
を作成します。グラウンド トゥルース加速度、角速度、方向を使用して IMU
を呼び出します。
IMU = imuSensor('accel-gyro', ... 'Gyroscope',gyroparams('RandomWalk',0.003,'ConstantBias',0.3), ... 'SampleRate',fs); [accelReadings, gyroReadingsBody] = IMU(accNED,angVelNED,qNED);
fuse
という imufilter
System object を作成します。モデル化された加速度計の読み取り値とジャイロスコープの読み取り値を使用して 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)')