慣性ナビゲーションの IMU と GPS フュージョン

この例では、無人航空機 (UAV) またはクワッドコプターに適した IMU と GPS のフュージョンアルゴリズムを作成する方法を示します。

この例では、加速度計、ジャイロスコープ、磁力計、および GPS を使用して、UAV の方向と位置を判別します。

シミュレーションの設定

サンプリングレートを設定します。典型的なシステムでは、加速度計およびジャイロスコープは、比較的高いサンプルレートで動作します。これらのセンサーからのデータをフュージョンアルゴリズムで処理することは、それほど複雑ではありません。逆に、GPS、および場合によっては磁力計は、比較的低いサンプルレートで動作し、その処理に関連する複雑度は高くなります。このフュージョンアルゴリズムでは、磁力計と GPS のサンプルは共に同じ低いレートで処理され、加速度計とジャイロスコープのサンプルは共に同じ高いレートで一緒に処理されます。

この構成をシミュレートするために、IMU (加速度計、ジャイロスコープ、磁力計) は 160 Hz でサンプリングされ、GPS は 1 Hz でサンプリングされます。磁力計の 160 サンプルのうち 1 つのサンプルのみフュージョンアルゴリズムに与えられるため、実際のシステムでは、磁力計はさらに低いレートでサンプリングされる可能性があります。

imuFs = 160;
gpsFs = 1;

% Define where on the Earth this simulated scenario takes place using the
% latitude, longitude and altitude.
refloc = [42.2825 -72.3430 53.0352];


% Validate that the |gpsFs| divides |imuFs|. This allows the sensor sample 
% rates to be simulated using a nested for loop without complex sample rate
% matching.

imuSamplesPerGPS = (imuFs/gpsFs);
assert(imuSamplesPerGPS == fix(imuSamplesPerGPS), ...
    'GPS sampling rate must be an integer factor of IMU sampling rate.');

フュージョンフィルター

IMU と GPS の測定を融合するフィルターを作成します。フュージョンフィルターは、拡張カルマンフィルターを使用して、向き (四元数として)、速度、位置、センサーバイアスおよび地磁気ベクトルを追跡します。

この insfilterMARG ではいくつかのメソッドを使って、predict、fusemag、fusegps を含むセンサーデータを処理します。predict メソッドは、IMU から加速度計およびジャイロスコープのサンプルを入力として取ります。加速度計とジャイロスコープがサンプリングされるたびに、predict メソッドを呼び出します。このメソッドは、加速度計とジャイロスコープに基づいて、1 タイムステップ先の状態を予測します。拡張カルマンフィルターの誤差の共分散は、ここで更新されます。

fusegps メソッドは、GPS のサンプルを入力として取ります。このメソッドは、さまざまなセンサー入力を不確かさに従って重み付けするカルマンゲインを計算することにより、GPS サンプルに基づいてフィルターの状態を更新します。誤差の共分散はここでも更新され、今回はカルマンゲインも使用されます。

fusemag メソッドは似ていますが、磁力計サンプルに基づいて状態、カルマンゲイン、および誤差の共分散を更新します。

insfilterMARG は加速度計とジャイロスコープのサンプルを入力として取りますが、これらは、それぞれデルタ速度とデルタ角を計算するために統合されます。フィルターは磁力計と以下の統合された信号のバイアスを追跡します。

fusionfilt = insfilterMARG;
fusionfilt.IMUSampleRate = imuFs;
fusionfilt.ReferenceLocation = refloc;

UAV の軌跡

この例では、UAV から記録された保存済みの軌跡をグラウンドトゥルースとして使用します。この軌跡はいくつかのセンサーシミュレーターに渡されて、シミュレートされた加速度計、ジャイロスコープ、磁力計、および GPS データストリームが計算されます。

% Load the "ground truth" UAV trajectory.
load LoggedQuadcopter.mat trajData;
trajOrient = trajData.Orientation;
trajVel = trajData.Velocity;
trajPos = trajData.Position;
trajAcc = trajData.Acceleration;
trajAngVel = trajData.AngularVelocity;

% Initialize the random number generator used in the simulation of sensor
% noise.
rng(1)

GPS センサー

指定のサンプルレートと参照場所で GPS を設定します。他のパラメーターは、出力信号内のノイズの性質を制御します。

gps = gpsSensor('UpdateRate', gpsFs);
gps.ReferenceLocation = refloc;     
gps.DecayFactor = 0.5;              % Random walk noise parameter 
gps.HorizontalPositionAccuracy = 1.6;   
gps.VerticalPositionAccuracy =  1.6;
gps.VelocityAccuracy = 0.1;

IMU センサー

通常、UAV は姿勢の推定に対して、統合された MARG センサー (磁力、角速度、重力) を使用します。MARG センサーをモデル化するには、加速度計、ジャイロスコープ、および磁力計を含む IMU センサーモデルを定義します。実際のアプリケーションでは、3 つのセンサーは、単一の集積回路によることも別々の回路に由来することもあります。ここで設定するプロパティ値は、低コストの MEMS センサーに典型的なものです。

imu = imuSensor('accel-gyro-mag', 'SampleRate', imuFs);
imu.MagneticField = [19.5281 -5.0741 48.0067];

% 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);

`insfilterMARG` の状態ベクトルの初期化

insfilterMARG は姿勢の状態を 22 要素のベクトルで追跡します。以下の状態があります。

     State                           Units        State Vector Index
 Orientation as a quaternion                      1:4
 Position (NED)                      m            5:7
 Velocity (NED)                      m/s          8:10 
 Delta Angle Bias (XYZ)              rad          11:13
 Delta Velocity Bias (XYZ)           m/s          14:16
 Geomagnetic Field Vector (NED)      uT           17:19
 Magnetometer Bias (XYZ)             uT           20:22

フィルターが短時間で適切な回答に収束するよう、フィルターの状態の初期化を補助するためにグラウンドトゥルースが使用されます。

% Initialize the states of the filter 

initstate = zeros(22,1);
initstate(1:4) = compact( meanrot(trajOrient(1:100))); 
initstate(5:7) = mean( trajPos(1:100,:), 1);
initstate(8:10) = mean( trajVel(1:100,:), 1);
initstate(11:13) =  imu.Gyroscope.ConstantBias./imuFs;
initstate(14:16) =  imu.Accelerometer.ConstantBias./imuFs;
initstate(17:19) =  imu.MagneticField;
initstate(20:22) = imu.Magnetometer.ConstantBias;

fusionfilt.State = initstate;

`insfilterMARG` の分散の初期化

insfilterMARG 測定ノイズは、どれだけのノイズがセンサーの読み取り値を破損しているかを記述します。これらの値は、imuSensor および gpsSensor パラメーターに基づきます。

プロセスノイズは、フィルター方程式が状態の進展をどの程度よく説明しているかを記述します。プロセスノイズは、パラメーターのスイープを使用してフィルターからの位置と向きの推定を一緒に最適化することにより、経験的に決定されます。

% Measurement noises
Rmag = 0.0862; % Magnetometer measurement noise
Rvel = 0.0051; % GPS Velocity measurement noise
Rpos = 5.169; % GPS Position measurement noise

% Process noises
fusionfilt.AccelerometerBiasNoise =  0.010716; 
fusionfilt.AccelerometerNoise = 9.7785; 
fusionfilt.GyroscopeBiasNoise = 1.3436e-14; 
fusionfilt.GyroscopeNoise =  0.00016528; 
fusionfilt.MagnetometerBiasNoise = 2.189e-11;
fusionfilt.GeomagneticVectorNoise = 7.67e-13;

% Initial error covariance
fusionfilt.StateCovariance = 1e-9*eye(22);

スコープの初期化

HelperScrollingPlotter スコープを使うと、変数の経時的なプロットが可能になります。ここでは、姿勢の誤差を追跡するために使用します。HelperPoseViewer スコープを使うと、フィルター推定値とグラウンドトゥルース姿勢を 3 次元で可視化できます。スコープによってシミュレーション速度は低下する場合があります。スコープを無効にするには、対応する論理変数を false に設定します。

useErrScope = true;  % Turn on the streaming error plot
usePoseView = true;  % Turn on the 3-D pose viewer

if useErrScope
    errscope = HelperScrollingPlotter(...
        'NumInputs', 4, ...
        'TimeSpan', 10, ...
        'SampleRate', imuFs, ...
        'YLabel', {'degrees', ...
        'meters', ...
        'meters', ...
        'meters'}, ...
        'Title', {'Quaternion Distance', ...
        'Position X Error', ...
        'Position Y Error', ...
        'Position Z Error'}, ...
        'YLimits', ...
        [ -3, 3
        -2, 2
        -2 2
        -2 2]);
end

if usePoseView
    posescope = HelperPoseViewer(...
        'XPositionLimits', [-15 15], ...
        'YPositionLimits', [-15, 15], ...
        'ZPositionLimits', [-10 10]);
end

シミュレーションループ

メインのシミュレーションループは、for ループが入れ子になっている while ループです。while ループは、GPS サンプルレートである gpsFs で実行されます。入れ子になった for ループは、IMU サンプルレートである imuFs で実行されます。スコープは IMU サンプルレートで更新されます。

% Loop setup - |trajData| has about 142 seconds of recorded data.
secondsToSimulate = 50; % simulate about 50 seconds
numsamples = secondsToSimulate*imuFs;

loopBound = floor(numsamples);
loopBound = floor(loopBound/imuFs)*imuFs; % ensure enough IMU Samples

% Log data for final metric computation.
pqorient = quaternion.zeros(loopBound,1);
pqpos = zeros(loopBound,3);

fcnt = 1;

while(fcnt <=loopBound)
    % |predict| loop at IMU update frequency.
    for ff=1:imuSamplesPerGPS
        % Simulate the IMU data from the current pose.
        [accel, gyro, mag] = imu(trajAcc(fcnt,:), trajAngVel(fcnt, :), ...
            trajOrient(fcnt));
        
        % Use the |predict| method to estimate the filter state based
        % on the simulated accelerometer and gyroscope signals.
        predict(fusionfilt, accel, gyro);
        
        % Acquire the current estimate of the filter states.
        [fusedPos, fusedOrient] = pose(fusionfilt);
        
        % Save the position and orientation for post processing.
        pqorient(fcnt) = fusedOrient;
        pqpos(fcnt,:) = fusedPos;
        
        % Compute the errors and plot.
        if useErrScope
            orientErr = rad2deg(dist(fusedOrient, ...
                trajOrient(fcnt) ));
            posErr = fusedPos - trajPos(fcnt,:); 
            errscope(orientErr, posErr(1), posErr(2), posErr(3));
        end
        
        % Update the pose viewer.
        if usePoseView
            posescope(pqpos(fcnt,:), pqorient(fcnt),  trajPos(fcnt,:), ...
                trajOrient(fcnt,:) );
        end
        fcnt = fcnt + 1;
    end
    
    % This next step happens at the GPS sample rate.
    % Simulate the GPS output based on the current pose.
    [lla, gpsvel] = gps( trajPos(fcnt,:), trajVel(fcnt,:) );
    
    % Correct the filter states based on the GPS data and magnetic
    % field measurements.
    fusegps(fusionfilt, lla, Rpos, gpsvel, Rvel);
    fusemag(fusionfilt, mag, Rmag);
 
end

MATLAB figure

誤差メトリクスの計算

位置と向きの推定値は、シミュレーション全体を通してログに記録されました。ここで、位置と向きの両方についてエンドツーエンドの平方根平均二乗誤差を計算します。

posd = pqpos(1:loopBound,:) - trajPos( 1:loopBound, :);

% For orientation, quaternion distance is a much better alternative to
% subtracting Euler angles, which have discontinuities. The quaternion
% distance can be computed with the |dist| function, which gives the
% angular difference in orientation in radians. Convert to degrees
% for display in the command window. 

quatd = rad2deg(dist(pqorient(1:loopBound), trajOrient(1:loopBound)) );

% Display RMS errors in the command window.
fprintf('\n\nEnd-to-End Simulation Position RMS Error\n');

End-to-End Simulation Position RMS Error

msep = sqrt(mean(posd.^2));
fprintf('\tX: %.2f , Y: %.2f, Z: %.2f   (meters)\n\n',msep(1), ...
    msep(2), msep(3));

	X: 0.50 , Y: 0.79, Z: 0.65   (meters)

fprintf('End-to-End Quaternion Distance RMS Error (degrees) \n');

End-to-End Quaternion Distance RMS Error (degrees)

fprintf('\t%.2f (degrees)\n\n', sqrt(mean(quatd.^2)));

	1.45 (degrees)