Entfernen Sie Verzerrungen bei der Winkelgeschwindigkeitsmessung
Dieses Beispiel zeigt, wie man mit imufilter die Gyroskopverzerrung aus einer IMU entfernt.
Verwenden Sie kinematicTrajectory, um eine Flugbahn mit zwei Teilen zu erstellen. Der erste Teil hat eine konstante Winkelgeschwindigkeit um die y- und z-Achsen. Der zweite Teil hat eine unterschiedliche Winkelgeschwindigkeit in allen drei Achsen.
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);
Erstellen Sie ein imuSensor System object ™, IMU, mit einem nicht idealen Gyroskop. Rufen Sie IMU mit der tatsächlichen Beschleunigung, Winkelgeschwindigkeit und Ausrichtung auf.
IMU = imuSensor('accel-gyro', ... 'Gyroscope',gyroparams('RandomWalk',0.003,'ConstantBias',0.3), ... 'SampleRate',fs); [accelReadings, gyroReadingsBody] = IMU(accNED,angVelNED,qNED);
Erstellen Sie ein imufilter System object, fuse. Rufen Sie fuse mit den modellierten Beschleunigungsmesser- und Gyroskopwerten auf.
fuse = imufilter('SampleRate',fs, 'GyroscopeDriftNoise', 1e-6); [~,angVelBodyRecovered] = fuse(accelReadings,gyroReadingsBody);
Zeichnen Sie die tatsächliche Winkelgeschwindigkeit, die Gyroskopwerte und die wiederhergestellte Winkelgeschwindigkeit für jede Achse auf.
Die vom imufilter zurückgegebene Winkelgeschwindigkeit kompensiert mit der Zeit den Effekt der Gyroskopverzerrung und konvergiert zur wahren Winkelgeschwindigkeit.
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)')
