Rimuovere la distorsione dalla misurazione della velocità angolare
Questo esempio mostra come rimuovere la polarizzazione del giroscopio da una IMU utilizzando imufilter.
Utilizzare kinematicTrajectory per creare una traiettoria con due parti. La prima parte ha una velocità angolare costante attorno agli assi y e z. La seconda parte ha una velocità angolare variabile su tutti e tre gli assi.
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);
Crea un imuSensor System object ™, IMU, con un giroscopio non ideale. Chiama IMU con i valori di accelerazione, velocità angolare e orientamento reali.
IMU = imuSensor('accel-gyro', ... 'Gyroscope',gyroparams('RandomWalk',0.003,'ConstantBias',0.3), ... 'SampleRate',fs); [accelReadings, gyroReadingsBody] = IMU(accNED,angVelNED,qNED);
Crea un imufilter System object, fuse . Chiama fuse con le letture modellate dell'accelerometro e del giroscopio.
fuse = imufilter('SampleRate',fs, 'GyroscopeDriftNoise', 1e-6); [~,angVelBodyRecovered] = fuse(accelReadings,gyroReadingsBody);
Rappresentare graficamente la velocità angolare di base, le letture del giroscopio e la velocità angolare recuperata per ciascun asse.
La velocità angolare restituita da imufilter compensa l'effetto della distorsione del giroscopio nel tempo e converge alla velocità angolare reale.
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)')
