非同期のセンサーからの姿勢推定
この例では、さまざまなレートでセンサーを融合して姿勢を推定する方法を示します。加速度計、ジャイロスコープ、磁力計、および GPS を使用して、円形のパスに沿って移動するビークルの向きと位置を判別します。Figure ウィンドウのコントロールを使用してセンサーのレートを変え、推定姿勢に対する影響を確認しながらセンサーのドロップアウトを試すことができます。
シミュレーションの設定
事前に記録されたセンサー データを読み込みます。センサー データは、waypointTrajectory クラスを使用して作成された円形の軌跡に基づいています。センサーの値は gpsSensor クラスと imuSensor クラスを使用して作成されたものです。ここで使用している CircularTrajectorySensorData.mat ファイルは generateCircularTrajSensorData 関数で生成できます。
ld = load('CircularTrajectorySensorData.mat'); Fs = ld.Fs; % maximum MARG rate gpsFs = ld.gpsFs; % maximum GPS rate ratio = Fs./gpsFs; refloc = ld.refloc; trajOrient = ld.trajData.Orientation; trajVel = ld.trajData.Velocity; trajPos = ld.trajData.Position; trajAcc = ld.trajData.Acceleration; trajAngVel = ld.trajData.AngularVelocity; accel = ld.accel; gyro = ld.gyro; mag = ld.mag; lla = ld.lla; gpsvel = ld.gpsvel;
フュージョン フィルター
IMU と GPS の測定を融合する insfilterAsync を作成します。このフュージョン フィルターでは、連続/離散の拡張カルマン フィルター (EKF) を使用して、向き (四元数として)、角速度、位置、速度、加速度、センサー バイアス、および地磁気ベクトルを追跡します。
この insfilterAsync には、センサー データを処理するメソッドがいくつかあります。fuseaccel、fusegyro、fusemag、および fusegps です。insfilterAsync で連続/離散の EKF を使用しているため、predict メソッドでフィルターをステップ実行して任意の時間まで進めることができます。
fusionfilt = insfilterAsync('ReferenceLocation', refloc);
insfilterAsync の状態ベクトルの初期化
insfilterAsync は姿勢の状態を 28 要素のベクトルで追跡します。以下の状態があります。
States Units Index Orientation (quaternion parts) 1:4 Angular Velocity (XYZ) rad/s 5:7 Position (NED) m 8:10 Velocity (NED) m/s 11:13 Acceleration (NED) m/s^2 14:16 Accelerometer Bias (XYZ) m/s^2 17:19 Gyroscope Bias (XYZ) rad/s 20:22 Geomagnetic Field Vector (NED) uT 23:25 Magnetometer Bias (XYZ) uT 26:28
フィルターが短時間で適切な回答に収束するよう、フィルターの状態の初期化を補助するためにグラウンド トゥルースが使用されます。
Nav = 100; initstate = zeros(28,1); initstate(1:4) = compact( meanrot(trajOrient(1:Nav))); initstate(5:7) = mean( trajAngVel(10:Nav,:), 1); initstate(8:10) = mean( trajPos(1:Nav,:), 1); initstate(11:13) = mean( trajVel(1:Nav,:), 1); initstate(14:16) = mean( trajAcc(1:Nav,:), 1); initstate(23:25) = ld.magField; % The gyroscope bias initial value estimate is low for the Z-axis. This is % done to illustrate the effects of fusing the magnetometer in the % simulation. initstate(20:22) = deg2rad([3.125 3.125 3.125]); fusionfilt.State = initstate;
insfilterAsync のプロセス ノイズ値の設定
フィルターで使用される運動モデルの不確かさをプロセス ノイズ分散として記述します。
fusionfilt.QuaternionNoise = 1e-2; fusionfilt.AngularVelocityNoise = 100; fusionfilt.AccelerationNoise = 100; fusionfilt.MagnetometerBiasNoise = 1e-7; fusionfilt.AccelerometerBiasNoise = 1e-7; fusionfilt.GyroscopeBiasNoise = 1e-7;
センサー データの融合に使用する測定ノイズ値の定義
センサーの測定値には、それぞれ何らかのノイズがあります。通常、これらの値はセンサーのデータシートに記載されています。
Rmag = 0.4; Rvel = 0.01; Racc = 610; Rgyro = 0.76e-5; Rpos = 3.4; fusionfilt.StateCovariance = diag(1e-3*ones(28,1));
スコープの初期化
HelperScrollingPlotter スコープを使うと、変数の経時的なプロットが可能になります。ここでは、姿勢の誤差を追跡するために使用します。PoseViewerWithSwitches スコープを使うと、フィルター推定値とグラウンド トゥルース姿勢を 3 次元で可視化できます。スコープによってシミュレーション速度は低下する場合があります。スコープを無効にするには、対応する論理変数を false に設定します。
useErrScope = true; % Turn on the streaming error plot. usePoseView = true; % Turn on the 3D pose viewer. if usePoseView posescope = PoseViewerWithSwitches(... 'XPositionLimits', [-30 30], ... 'YPositionLimits', [-30, 30], ... 'ZPositionLimits', [-10 10]); end f = gcf; if useErrScope errscope = HelperScrollingPlotter(... 'NumInputs', 4, ... 'TimeSpan', 10, ... 'SampleRate', Fs, ... 'YLabel', {'degrees', ... 'meters', ... 'meters', ... 'meters'}, ... 'Title', {'Quaternion Distance', ... 'Position X Error', ... 'Position Y Error', ... 'Position Z Error'}, ... 'YLimits', ... [ -1, 30 -2, 2 -2 2 -2 2]); end
シミュレーション ループ
フュージョン アルゴリズムのシミュレーションで、センサーのサンプル レートを変えたときの影響を検証できます。さらに、対応するチェックボックスをオフにして個々のセンサーの融合を回避できます。これを使用してセンサーのドロップアウトをシミュレートできます。
構成によっては劇的な結果になるものがあります。たとえば、GPS センサーをオフにすると、位置の推定のドリフトが速くなります。磁力計センサーをオフにすると、推定のローテーションが速すぎるために、向きの推定がグラウンド トゥルースから徐々に逸脱していきます。一方、ジャイロスコープをオフにして磁力計をオンにすると、推定される向きにゆがみが生じ、両方のセンサーを使用した場合に得られるような滑らかさに欠けたものになります。
すべてのセンサーをオンにしても、それらを最低のレートで動作するように設定している場合は、グラウンド トゥルースから明らかに逸脱した推定が生成され、その後にセンサーを融合した時点でより正確な結果が得られます。これは、実行時推定の誤差の HelperScrollingPlotter で最もよく見られます。
メインのシミュレーションは 100 Hz で実行されます。それぞれの反復で Figure ウィンドウのチェックボックスが検証され、センサーが有効になっていれば、そのセンサーのデータが適切なレートで融合されます。
for ii=1:size(accel,1) fusionfilt.predict(1./Fs); % Fuse Accelerometer if (f.UserData.Accelerometer) && ... mod(ii, fix(Fs/f.UserData.AccelerometerSampleRate)) == 0 fusionfilt.fuseaccel(accel(ii,:), Racc); end % Fuse Gyroscope if (f.UserData.Gyroscope) && ... mod(ii, fix(Fs/f.UserData.GyroscopeSampleRate)) == 0 fusionfilt.fusegyro(gyro(ii,:), Rgyro); end % Fuse Magnetometer if (f.UserData.Magnetometer) && ... mod(ii, fix(Fs/f.UserData.MagnetometerSampleRate)) == 0 fusionfilt.fusemag(mag(ii,:), Rmag); end % Fuse GPS if (f.UserData.GPS) && mod(ii, fix(Fs/f.UserData.GPSSampleRate)) == 0 fusionfilt.fusegps(lla(ii,:), Rpos, gpsvel(ii,:), Rvel); end % Plot the pose error [p,q] = pose(fusionfilt); posescope(p, q, trajPos(ii,:), trajOrient(ii)); orientErr = rad2deg(dist(q, trajOrient(ii) )); posErr = p - trajPos(ii,:); errscope(orientErr, posErr(1), posErr(2), posErr(3)); end
まとめ
insfilterAsync では、サンプル レートをさまざまに変えることができます。推定出力の品質は個々のセンサー フュージョンのレートに大きく依存します。センサーのドロップアウトは、いずれも出力に大きく影響します。
