IMU、磁力計、高度計を使用した向きと高度の推定

この例では、3 軸加速度計、3 軸ジャイロスコープ、3 軸磁力計 (一般に MARG センサー (磁気、角速度、重力) と総称)、および 1 軸高度計からのデータを融合して向きと高度を推定する方法を示します。

シミュレーションの設定

このシミュレーションでは、複数のレートでセンサー データを処理します。一般に、IMU (加速度計とジャイロスコープ) が最も高いレートで動作します。磁力計は一般に IMU よりも低いレートで動作し、高度計が最も低いレートで動作します。サンプル レートを変更すると、フュージョン アルゴリズムの一部がより高い頻度で実行されるようになり、パフォーマンスに影響することがあります。

% Set the sampling rate for IMU sensors, magnetometer, and altimeter.
imuFs = 100;
altFs = 10;
magFs = 25;
imuSamplesPerAlt = fix(imuFs/altFs);
imuSamplesPerMag = fix(imuFs/magFs);

% Set the number of samples to simulate. 
N = 1000; 

% Construct object for other helper functions.
hfunc = Helper10AxisFusion;

軌跡の定義

センサー ボディは 3 軸すべてを中心に回転しながら、位置が垂直方向に振動します。振動の振幅はシミュレーションを続けるほど大きくなります。

% Define the initial state of the sensor body
initPos = [0, 0, 0];       % initial position (m)
initVel = [0, 0, -1];      % initial linear velocity (m/s)
initOrient = ones(1, 'quaternion'); 

% Define the constant angular velocity for rotating the sensor body
% (rad/s).
angVel = [0.34 0.2 0.045];  

% Define the acceleration required for simple oscillating motion of the
% sensor body.
fc = 0.2; 
t = 0:1/imuFs:(N-1)/imuFs;
a = 1; 
oscMotionAcc = sin(2*pi*fc*t);
oscMotionAcc = hfunc.growAmplitude(oscMotionAcc);

% Construct the trajectory object
traj = kinematicTrajectory('SampleRate', imuFs, ...
    'Velocity', initVel, ...
    'Position', initPos, ...
    'Orientation', initOrient);

センサー構成

加速度計、ジャイロスコープ、および磁力計は imuSensor を使用してシミュレートされます。高度計は altimeterSensor を使用してモデル化されます。センサー構成で使用される値は、実際の MEMS センサーの値に対応しています。

imu = imuSensor('accel-gyro-mag', 'SampleRate', imuFs);

% Accelerometer
imu.Accelerometer.MeasurementRange =  19.6133;
imu.Accelerometer.Resolution = 0.0023928;
imu.Accelerometer.ConstantBias = 0.19;
imu.Accelerometer.NoiseDensity = 0.0012356;

% Gyroscope
imu.Gyroscope.MeasurementRange = deg2rad(250);
imu.Gyroscope.Resolution = deg2rad(0.0625);
imu.Gyroscope.ConstantBias = deg2rad(3.125);
imu.Gyroscope.AxesMisalignment = 1.5;
imu.Gyroscope.NoiseDensity = deg2rad(0.025);

% Magnetometer
imu.Magnetometer.MeasurementRange = 1000;
imu.Magnetometer.Resolution = 0.1;
imu.Magnetometer.ConstantBias = 100;
imu.Magnetometer.NoiseDensity = 0.3/sqrt(50);

% altimeter
altimeter = altimeterSensor('UpdateRate', altFs, 'NoiseDensity', 2*0.1549);

フュージョン フィルター

ahrs10filter を作成して構成します。

fusionfilt = ahrs10filter; 
fusionfilt.IMUSampleRate = imuFs;

フュージョン フィルターの初期値を設定します。

initstate = zeros(18,1);
initstate(1:4) = compact(initOrient);
initstate(5) = initPos(3);
initstate(6) = initVel(3);
initstate(7:9) = imu.Gyroscope.ConstantBias/imuFs;
initstate(10:12) = imu.Accelerometer.ConstantBias/imuFs;
initstate(13:15) = imu.MagneticField;
initstate(16:18) = imu.Magnetometer.ConstantBias;
fusionfilt.State = initstate;

フュージョン フィルターの状態共分散行列を初期化します。初期状態にグラウンド トゥルースを使用しているため、推定に誤差はほとんどないはずです。

icv = diag([1e-8*[1 1 1 1 1 1 1], 1e-3*ones(1,11)]);
fusionfilt.StateCovariance = icv;

磁力計と高度計の測定ノイズは、ahrs10filter の内部のカルマン フィルターで使用されているセンサーに関連する観測ノイズです。これらの値は、通常はセンサーのデータシートから得られます。

magNoise =  2*(imu.Magnetometer.NoiseDensity(1).^2)*imuFs;
altimeterNoise = 2*(altimeter.NoiseDensity).^2 * altFs;

フィルターのプロセス ノイズを使用して、フィルターを目的の性能に調整します。

fusionfilt.AccelerometerNoise = [1e-1 1e-1 1e-4];
fusionfilt.AccelerometerBiasNoise = 1e-8;
fusionfilt.GeomagneticVectorNoise = 1e-12;
fusionfilt.MagnetometerBiasNoise = 1e-12;
fusionfilt.GyroscopeNoise = 1e-12;

追加のシミュレーション オプション: ビューアー

このシミュレーションでは、既定ではシミュレーションの最後に推定誤差をプロットします。シミュレーションの実行中に推定される位置と向きの両方をグラウンド トゥルースと一緒に表示するには、usePoseViewer 変数を true に設定します。

usePoseViewer = false;

シミュレーション ループ

q = initOrient;
firstTime = true;

actQ = zeros(N,1, 'quaternion');
expQ = zeros(N,1, 'quaternion');
actP = zeros(N,1);
expP = zeros(N,1);

for ii = 1: N
    % Generate a new set of samples from the trajectory generator 
    accBody = rotateframe(q, [0 0 +oscMotionAcc(ii)]);
    omgBody = rotateframe(q, angVel);
    [pos, q, vel, acc] = traj(accBody, omgBody);
    
    % Feed the current position and orientation to the imuSensor object
    [accel, gyro, mag] = imu(acc, omgBody, q);
    fusionfilt.predict(accel, gyro);
   
    % Fuse magnetometer samples at the magnetometer sample rate
    if ~mod(ii,imuSamplesPerMag)
        fusemag(fusionfilt, mag, magNoise);
    end

    % Sample and fuse the altimeter output at the altimeter sample rate
    if ~mod(ii,imuSamplesPerAlt)
        altHeight = altimeter(pos);
       
        % Use the |fusealtimeter| method to update the fusion filter with
        % the altimeter output.
        fusealtimeter(fusionfilt,altHeight,altimeterNoise);
    end

    % Log the actual orientation and position
    [actP(ii), actQ(ii)] = pose(fusionfilt);
    
    % Log the expected orientation and position
    expQ(ii) = q;
    expP(ii) = pos(3);

    if usePoseViewer
        hfunc.view(actP(ii), actQ(ii),expP(ii), expQ(ii)); %#ok<*UNRCH>
    end

end

フィルター性能のプロット

フィルターの性能をプロットします。次の表示は、四元数距離を使用した向きの誤差と高度の誤差を示しています。

hfunc.plotErrs(actP, actQ, expP, expQ);

まとめ

この例は、ahrs10filter を使用して高度と向きの 10 軸センサー フュージョンを実行する方法を示したものです。