Позируйте от IMU, GPS и монокулярной визуальной одометрии (MVO)
Объект ErrorStateIMUGPSFuser реализует сплав датчика IMU, GPS и данных о монокулярной визуальной одометрии (MVO), чтобы оценить положение в ссылочном кадре NED. Фильтр использует вектор состояния с 17 элементами, чтобы отследить ориентацию quaternion, скорость, положение, смещения датчика IMU и масштабный коэффициент MVO. Объект ErrorStateIMUGPSFuser использует Фильтр Калмана состояния ошибки, чтобы оценить эти количества.
Создайте ErrorStateIMUGPSFuser, чтобы плавить IMU, GPS и данные MVO с помощью insfilter:
filt = insfilter('errorstate');predict | Обновите состояния с помощью данных о гироскопе и акселерометра |
fusegps | Правильные состояния с помощью данных о GPS |
fusemvo | Правильные состояния с помощью монокулярной визуальной одометрии |
correct | Правильные состояния с помощью прямых измерений состояния |
pose | Текущая ориентация и оценка положения |
reset | Сбросьте внутренние состояния |
stateinfo | Отобразите информацию о векторе состояния |
Загрузите записанные данные наземного транспортного средства после круговой траектории. Файл .mat содержит IMU и измерения датчика GPS и наземную ориентацию истины и положение.
load('loggedGroundVehicleCircle.mat', ... 'imuFs','localOrigin', ... 'initialStateCovariance', ... 'accelData','gyroData', ... 'gpsFs','gpsLLA','Rpos','gpsVel','Rvel', ... 'trueOrient','truePos');
Создайте фильтр INS, чтобы плавить IMU и данные о GPS с помощью Фильтра Калмана состояния ошибки.
initialState = [compact(trueOrient(1)),truePos(1,:),-6.8e-3,2.5002,0,zeros(1,6),1].';
filt = insfilter('ErrorState');
filt.IMUSampleRate = imuFs;
filt.ReferenceLocation = localOrigin;
filt.State = initialState;
filt.StateCovariance = initialStateCovariance;
Предварительно выделите переменные для положения и ориентации. Выделите переменную для индексации в данные о GPS.
numIMUSamples = size(accelData,1);
estOrient = ones(numIMUSamples,1,'quaternion');
estPos = zeros(numIMUSamples,3);
gpsIdx = 1;
Плавьте акселерометр, гироскоп и данные о GPS. Внешний цикл предсказывает фильтр вперед на уровне самой быстрой частоты дискретизации (частота дискретизации IMU).
for idx = 1:numIMUSamples % Use predict to estimate the filter state based on the accelData and % gyroData arrays. predict(filt,accelData(idx,:),gyroData(idx,:)); % GPS data is collected at a lower sample rate than IMU data. Fuse GPS % data at the lower rate. if mod(idx, imuFs / gpsFs) == 0 % Correct the filter states based on the GPS data. fusegps(filt,gpsLLA(gpsIdx,:),Rpos,gpsVel(gpsIdx,:),Rvel); gpsIdx = gpsIdx + 1; end % Log the current pose estimate [estPos(idx,:), estOrient(idx,:)] = pose(filt); end
Вычислите ошибки RMS между известным истинным положением и ориентацией и выводом от фильтра состояния ошибки.
pErr = truePos - estPos; qErr = rad2deg(dist(estOrient,trueOrient)); pRMS = sqrt(mean(pErr.^2)); qRMS = sqrt(mean(qErr.^2)); fprintf('Position RMS Error\n'); fprintf('\tX: %.2f, Y: %.2f, Z: %.2f (meters)\n\n',pRMS(1),pRMS(2),pRMS(3)); fprintf('Quaternion Distance RMS Error\n'); fprintf('\t%.2f (degrees)\n\n',qRMS);
Position RMS Error X: 0.40, Y: 0.24, Z: 0.05 (meters) Quaternion Distance RMS Error 0.30 (degrees)
Визуализируйте истинное положение и предполагаемое положение.
plot(truePos(:,1),truePos(:,2),estPos(:,1),estPos(:,2),'r:','LineWidth',2) grid on axis square xlabel('N (m)') ylabel('E (m)') legend('Ground Truth','Estimation')
