Main Content

지상 이동체의 위치와 방향 추정하기

이 예제는 IMU(관성 측정 장치)와 GPS(위성 측위 시스템) 수신기에서 제공되는 데이터를 융합하여 지상 이동체의 위치와 방향을 추정하는 방법을 보여줍니다.

시뮬레이션 설정

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

이와 같은 구성을 시뮬레이하기 위해 IMU(가속도계와 자이로스코프)는 100Hz로 샘플링되고 GPS는 10Hz로 샘플링됩니다.

imuFs = 100;
gpsFs = 10;

% Define where on the Earth this simulation takes place using latitude, 
% longitude, and altitude (LLA) coordinates.
localOrigin = [42.2825 -71.343 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 측정값을 융합하는 필터를 만듭니다. 융합 필터는 방향(쿼터니언으로 지정), 위치, 속도, 센서 편향을 추적하기 위해 확장 칼만 필터를 사용합니다.

insfilterNonholonomic 객체에는 predictfusegps라는 두 가지 기본 메서드가 있습니다. predict 메서드는 IMU의 가속도계 샘플과 자이로스코프 샘플을 입력값으로 사용합니다. 가속도계와 자이로스코프가 샘플링될 때마다 predict 메서드를 호출합니다. 이 메서드는 가속도계와 자이로스코프를 기반으로 하나 앞의 시간 스텝의 상태를 예측합니다. 확장 칼만 필터의 오차 공분산은 이 단계에서 업데이트됩니다.

fusegps 메서드는 GPS 샘플을 입력값으로 사용합니다. 이 메서드는 불확실성에 따라 다양한 센서 입력값에 가중치를 부여하는 칼만 이득을 계산하여 GPS 샘플을 기반으로 필터 상태를 업데이트합니다. 오차 공분산도 이 단계에서 업데이트되는데, 이번에는 칼만 이득도 사용합니다.

insfilterNonholonomic 객체에는 IMUSampleRateDecimationFactor라는 두 가지 기본 속성이 있습니다. 지상 이동체는 지면에 부딪혀 튀어오르거나 지면에서 미끄러지지 않는다고 가정하는 두 가지 속도 제약 조건을 가지고 있습니다. 이 제약 조건은 확장 칼만 필터 업데이트 방정식을 사용하여 적용됩니다. 이러한 업데이트는 IMUSampleRate/DecimationFactor Hz의 레이트로 필터 상태에 적용됩니다.

gndFusion = insfilterNonholonomic('ReferenceFrame', 'ENU', ...
    'IMUSampleRate', imuFs, ...
    'ReferenceLocation', localOrigin, ...
    'DecimationFactor', 2);

지상 이동체의 궤적 생성

waypointTrajectory 객체는 지정된 샘플링 레이트, 웨이포인트, 도착 시간, 방향을 기준으로 자세를 계산합니다. 지상 이동체에 대한 원형 궤적의 파라미터를 지정하십시오.

% Trajectory parameters
r = 8.42; % (m)
speed = 2.50; % (m/s)
center = [0, 0]; % (m)
initialYaw = 90; % (degrees)
numRevs = 2;

% Define angles theta and corresponding times of arrival t.
revTime = 2*pi*r / speed;
theta = (0:pi/2:2*pi*numRevs).';
t = linspace(0, revTime*numRevs, numel(theta)).';

% Define position.
x = r .* cos(theta) + center(1);
y = r .* sin(theta) + center(2);
z = zeros(size(x));
position = [x, y, z];

% Define orientation.
yaw = theta + deg2rad(initialYaw);
yaw = mod(yaw, 2*pi);
pitch = zeros(size(yaw));
roll = zeros(size(yaw));
orientation = quaternion([yaw, pitch, roll], 'euler', ...
    'ZYX', 'frame');

% Generate trajectory.
groundTruth = waypointTrajectory('SampleRate', imuFs, ...
    'Waypoints', position, ...
    'TimeOfArrival', t, ...
    'Orientation', orientation);

% Initialize the random number generator used to simulate sensor noise.
rng('default');

GPS 수신기

지정된 샘플 레이트와 기준 위치에서 GPS를 설정합니다. 그 외 파라미터는 출력 신호의 잡음 특성을 제어합니다.

gps = gpsSensor('UpdateRate', gpsFs, 'ReferenceFrame', 'ENU');
gps.ReferenceLocation = localOrigin;
gps.DecayFactor = 0.5;                % Random walk noise parameter 
gps.HorizontalPositionAccuracy = 1.0;   
gps.VerticalPositionAccuracy =  1.0;
gps.VelocityAccuracy = 0.1;

IMU 센서

일반적으로 지상 이동체는 자세 추정을 위해 6축 IMU 센서를 사용합니다. IMU 센서를 모델링하기 위해 가속도계와 자이로스코프를 포함하는 IMU 센서 모델을 정의합니다. 실제 응용 시 이 두 센서는 단일 집적 회로 또는 여러 개의 분리된 집적 회로가 될 수 있습니다. 여기에 설정한 속성값은 저렴한 MEMS 센서에서 일반적으로 사용됩니다.

imu = imuSensor('accel-gyro', ...
    'ReferenceFrame', 'ENU', 'SampleRate', imuFs);

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

% Gyroscope
imu.Gyroscope.MeasurementRange = deg2rad(250);
imu.Gyroscope.Resolution = deg2rad(0.0625);
imu.Gyroscope.NoiseDensity = deg2rad(0.025);

insfilterNonholonomic의 상태 초기화하기

상태는 다음과 같습니다.

States                            Units    Index
Orientation (quaternion parts)             1:4  
Gyroscope Bias (XYZ)              rad/s    5:7  
Position (NED)                    m        8:10 
Velocity (NED)                    m/s      11:13
Accelerometer Bias (XYZ)          m/s^2    14:16

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

% Get the initial ground truth pose from the first sample of the trajectory
% and release the ground truth trajectory to ensure the first sample is not 
% skipped during simulation.
[initialPos, initialAtt, initialVel] = groundTruth();
reset(groundTruth);

% Initialize the states of the filter
gndFusion.State(1:4) = compact(initialAtt).';
gndFusion.State(5:7) = imu.Gyroscope.ConstantBias;
gndFusion.State(8:10) = initialPos.';
gndFusion.State(11:13) = initialVel.';
gndFusion.State(14:16) = imu.Accelerometer.ConstantBias;

insfilterNonholonomic의 분산 초기화하기

측정 잡음은 얼마나 많은 잡음이 gpsSensor 파라미터를 기반으로 하는 GPS 측정값을 손상시키고 얼마나 많은 불확실성이 이동체 동적 모델에 존재하는지를 나타냅니다.

공정 잡음은 필터 방정식이 상태 변화를 얼마나 잘 묘사하고 있는지를 나타냅니다. 또한 공정 잡음은 필터에서의 위치 추정값과 방향 추정값을 함께 최적화할 수 있도록 파라미터 스윕을 사용하여 경험적으로 결정됩니다.

% Measurement noises
Rvel = gps.VelocityAccuracy.^2;
Rpos = gps.HorizontalPositionAccuracy.^2;

% The dynamic model of the ground vehicle for this filter assumes there is
% no side slip or skid during movement. This means that the velocity is 
% constrained to only the forward body axis. The other two velocity axis 
% readings are corrected with a zero measurement weighted by the 
% |ZeroVelocityConstraintNoise| parameter.
gndFusion.ZeroVelocityConstraintNoise = 1e-2;

% Process noises
gndFusion.GyroscopeNoise = 4e-6;
gndFusion.GyroscopeBiasNoise = 4e-14;
gndFusion.AccelerometerNoise = 4.8e-2;
gndFusion.AccelerometerBiasNoise = 4e-14;

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

스코프 초기화하기

HelperScrollingPlotter 스코프를 사용하여 시간 경과에 따라 변수를 플로팅할 수 있습니다. 여기서는 자세의 오차를 추적하는 데 사용됩니다. 실측HelperPoseViewer 스코프를 통해 필터 추정값과 실측 자세를 3차원으로 시각화할 수 있습니다. 스코프를 사용할 경우 시뮬레이션이 느려질 수 있습니다. 스코프를 비활성화하려면 해당 논리형 변수를 false로 설정하십시오.

useErrScope = true; % Turn on the streaming error plot
usePoseView = true;  % Turn on the 3D 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', ...
            [-1, 1
             -1, 1
             -1, 1
             -1, 1]);
end

if usePoseView
    viewer = HelperPoseViewer( ...
        'XPositionLimits', [-15, 15], ...
        'YPositionLimits', [-15, 15], ...
        'ZPositionLimits', [-5, 5], ...
        'ReferenceFrame', 'ENU');
end

시뮬레이션 루프

기본 시뮬레이션 루프는 중첩된 for 루프가 있는 while 루프입니다. while 루프는 GPS 측정 레이트인 gpsFs로 실행됩니다. 중첩된 for 루프는 IMU 샘플 레이트인 imuFs로 실행됩니다. 스코프는 IMU 샘플 레이트로 업데이트됩니다.

totalSimTime = 30; % seconds

% Log data for final metric computation.
numGPSSamples = floor(min(t(end), totalSimTime) * gpsFs);
numSamples = numGPSSamples*imuSamplesPerGPS;
truePosition = zeros(numSamples,3);
trueOrientation = quaternion.zeros(numSamples,1);
estPosition = zeros(numSamples,3);
estOrientation = quaternion.zeros(numSamples,1);

idx = 0;

for sampleIdx = 1:numGPSSamples
    % Predict loop at IMU update frequency.
    for i = 1:imuSamplesPerGPS
        if ~isDone(groundTruth)
            idx = idx + 1;
            
            % Simulate the IMU data from the current pose.
            [truePosition(idx,:), trueOrientation(idx,:), ...
                trueVel, trueAcc, trueAngVel] = groundTruth();
            [accelData, gyroData] = imu(trueAcc, trueAngVel, ...
                trueOrientation(idx,:));
            
            % Use the predict method to estimate the filter state based
            % on the accelData and gyroData arrays.
            predict(gndFusion, accelData, gyroData);
            
            % Log the estimated orientation and position.
            [estPosition(idx,:), estOrientation(idx,:)] = pose(gndFusion);
            
            % Compute the errors and plot.
            if useErrScope
                orientErr = rad2deg( ...
                    dist(estOrientation(idx,:), trueOrientation(idx,:)));
                posErr = estPosition(idx,:) - truePosition(idx,:);
                errscope(orientErr, posErr(1), posErr(2), posErr(3));
            end

            % Update the pose viewer.
            if usePoseView
                viewer(estPosition(idx,:), estOrientation(idx,:), ...
                    truePosition(idx,:), trueOrientation(idx,:));
            end
        end
    end
    
    if ~isDone(groundTruth)
        % This next step happens at the GPS sample rate.
        % Simulate the GPS output based on the current pose.
        [lla, gpsVel] = gps(truePosition(idx,:), trueVel);

        % Update the filter states based on the GPS data.
        fusegps(gndFusion, lla, Rpos, gpsVel, Rvel);
    end
end

Figure Scrolling Plotter contains 4 axes objects. Axes object 1 with title Position Z Error, xlabel Time (s), ylabel meters contains an object of type line. Axes object 2 with title Position Y Error, xlabel Time (s), ylabel meters contains an object of type line. Axes object 3 with title Position X Error, xlabel Time (s), ylabel meters contains an object of type line. Axes object 4 with title Quaternion Distance, xlabel Time (s), ylabel degrees contains an object of type line.

MATLAB figure

오차 메트릭 계산

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

posd = estPosition - truePosition;

% 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(estOrientation, trueOrientation));

% 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: 1.16 , Y: 0.98, Z: 0.03   (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)));
	0.11 (degrees)