관성 항법을 위한 IMU 및 GPS 융합

이 예제는 UAV(무인 항공기) 또는 쿼드콥터에 적합한 IMU + GPS 융합 알고리즘을 작성하는 방법을 보여줍니다.

이 예제에서는 UAV의 방향과 위치를 결정하기 위해 가속도계, 자이로스코프, 자력계, GPS를 사용합니다.

시뮬레이션 설정

샘플링 레이트를 설정합니다. 일반적인 시스템에서 가속도계와 자이로스코프는 상대적으로 높은 샘플 레이트로 실행됩니다. 융합 알고리즘에서 이러한 센서의 데이터를 처리하는 과정은 비교적 덜 복잡합니다. 반면에, 경우에 따라 자력계도 마찬가지지만 GPS는 상대적으로 낮은 샘플 레이트로 실행되며 처리하는 과정이 꽤 복잡한 편입니다. 이러한 융합 알고리즘에서, 자력계와 GPS 샘플은 동일한 낮은 레이트로 함께 처리되고 가속도계와 자이로스코프 샘플은 동일한 높은 레이트로 함께 처리됩니다.

이와 같은 구성을 시뮬레이션하기 위해 IMU(가속도계, 자이로스코프, 자력계)는 160Hz로 샘플링되고 GPS는 1Hz로 샘플링됩니다. 자력계의 160개 샘플마다 하나만 융합 알고리즘에 제공되므로 실제 시스템에서 자력계는 훨씬 낮은 속도로 샘플링할 수 있습니다.

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 메서드를 호출합니다. 이 메서드는 가속도계와 자이로스코프를 기반으로 하나 앞의 시간 스텝의 상태를 예측합니다. 확장 칼만 필터의 오차 공분산은 이 시점에 업데이트됩니다.

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 센서 모델을 정의합니다. 실제 응용 시 이 세 센서는 단일 집적 회로 또는 여러 개의 분리된 집적 회로가 될 수 있습니다. 여기에 설정한 속성값은 저렴한 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

필터 상태를 초기화하는 데 도움이 되도록 ground truth를 사용하며, 따라서 필터가 적절한 해답으로 빠르게 수렴됩니다.

% 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

오차 메트릭 계산

시뮬레이션 전체에 걸쳐 위치 추정값과 방향 추정값이 기록되었습니다. 이제 위치와 방향에 대한 RMS 오차를 전체적으로 계산합니다.

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)