除去角速度偏差测量
这个例子显示了如何删除从一个IMU使用陀螺仪的偏见imufilter。
使用kinematicTrajectory创建一个轨迹与两个部分。第一部分有一个恒定的角速度有关y- - -z相互重合。第二部分有一个不同的三个轴的角速度。
时间= 60 * 8;fs = 20;numSamples = * fs持续时间;rng (“默认”)%种子繁殖RNG嘈杂的传感器测量。initialAngVel = (0、0.5、0.25);finalAngVel = (-0.2, 0.6, 0.5);constantAngVel = repmat (initialAngVel,地板(numSamples / 2), 1);varyingAngVel = [linspace (initialAngVel (1) finalAngVel(1),装天花板(numSamples / 2)。”…finalAngVel linspace (initialAngVel(2),(2),装天花板(numSamples / 2)。”…linspace (initialAngVel (3) finalAngVel(3),装天花板(numSamples / 2)。');angVelBody = [constantAngVel;varyingAngVel];accBody = 0 (numSamples, 3);traj = kinematicTrajectory (“SampleRate”fs);[qn, ~ ~, accNED angVelNED] = traj (accBody angVelBody);
创建一个imuSensor系统对象™,IMU,非理想的陀螺仪。调用IMU真实与加速度、角速度和取向。
IMU = imuSensor (“accel-gyro”,…“陀螺”gyroparams (“随机散步”,0.003,“ConstantBias”0.3),…“SampleRate”fs);[accelReadings, gyroReadingsBody] = IMU (accNED、angVelNED qn);
创建一个imufilter系统对象,保险丝。调用保险丝加速度计读数和陀螺仪数据建模。
保险丝= imufilter (“SampleRate”fs,“GyroscopeDriftNoise”1 e-6);[~,angVelBodyRecovered] =保险丝(accelReadings gyroReadingsBody);
情节真实的角速度,陀螺仪数据,恢复为每个轴角速度。
返回的角速度imufilter补偿陀螺偏见的影响随着时间的推移和收敛于真实的角速度。
时间= (0:numSamples-1) / fs;图(1)图(时间,angVelBody (: 1),…时间,gyroReadingsBody (: 1),…时间,angVelBodyRecovered(: 1)标题(“轴”)传说(“真正的角速度”,…“陀螺仪数据”,…“恢复角速度”)ylabel (“角速度(rad / s)”)
图(2)图(时间,angVelBody (:, 2),…时间,gyroReadingsBody (:, 2),…时间,angVelBodyRecovered(: 2)标题(“轴”)ylabel (“角速度(rad / s)”)
图(3)图(时间,angVelBody (:, 3),…时间,gyroReadingsBody (:, 3),…时间,angVelBodyRecovered(: 3)标题(z轴的)ylabel (“角速度(rad / s)”)包含(“时间(s)”)
