Определение позы с помощью инерционных датчиков и GPS

Функция Sensor Fusion and Tracking Toolbox™ позволяет сплавлять данные, считываемые из IMU и GPS, для оценки позы. Используйте insfilter для создания комбинированного фильтра INS/GPS, подходящего для вашей системы:

MARGGPSFuser - Оцените позу, используя магнитометр, гироскоп, акселерометр и данные GPS.
NHConstrainedIMUGPSFuser - Оцените позу с помощью гироскопа, акселерометра и данных GPS. Этот алгоритм использует неголономные ограничения.

В этом учебном пособии представлен обзор инерционного слияния датчиков с GPS в Sensor Fusion и Tracking Toolbox.

Сведения о моделировании инерционных датчиков и GPS см. в разделе Модель IMU, GPS и INS/GPS. Чтобы узнать, как создать движение «земля-истина», которое управляет моделями датчиков, см. раздел waypointTrajectory и kinematicTrajectory.

Также можно сплавить данные инерциального датчика без GPS для оценки ориентации. См. раздел Определение ориентации с помощью инерционных датчиков.

Инерционный датчик предохранителя и данные GPS

Инерциальная навигационная система (ИНС) состоит из датчиков, которые обнаруживают поступательное движение (акселерометры), вращение (гироскопы), а в некоторых системах магнитные поля (магнитометры). Благодаря непрерывному сплавлению данных датчиков ИНС может полностью учитывать позу платформы без внешних датчиков. Однако оценка позы INS обычно уменьшается с течением времени и должна быть скорректирована с использованием внешней ссылки, такой как показания GPS. Общие конфигурации для слияния INS/GPS включают MARG + GPS для летательных аппаратов и акселерометр + гироскоп + GPS с неголономными ограничениями для наземных транспортных средств.

Предохранитель MARG и GPS

Магнитная, угловая и гравитационная система (MARG) состоит из магнитометра, гироскопа и акселерометра. Для слияния данных MARG и GPS создайте insfilterMARG объект:

FUSE = insfilterMARG

FUSE = 

  insfilterMARG with properties:

        IMUSampleRate: 100               Hz         
    ReferenceLocation: [0 0 0]           [deg deg m]
                State: [22x1 double]                
      StateCovariance: [22x22 double]               

   Multiplicative Process Noise Variances
            GyroscopeNoise: [1e-09 1e-09 1e-09]       (rad/s)²
        AccelerometerNoise: [0.0001 0.0001 0.0001]    (m/s²)² 
        GyroscopeBiasNoise: [1e-10 1e-10 1e-10]       (rad/s)²
    AccelerometerBiasNoise: [0.0001 0.0001 0.0001]    (m/s²)² 

   Additive Process Noise Variances
    GeomagneticVectorNoise: [1e-06 1e-06 1e-06]    uT²
     MagnetometerBiasNoise: [0.1 0.1 0.1]          uT²

insfilterMARG использует расширенный фильтр Калмана со следующими методами:

predict - Обновление состояний с использованием данных акселерометра и гироскопа
fusemag - Правильные состояния с использованием данных магнитометра
fusegps - Правильные состояния с использованием данных GPS

Обычно данные акселерометра и гироскопа получают с более высокой скоростью, чем данные магнитометра и GPS. Для вызова можно использовать вложенные циклы predict, fusemag, и fusegps по разным ставкам.

accelData   = [0 0 9.8];
gyroData    = [0 0 0];
magData     = [19.535 -5.109 47.930];
magCov      = 0;
position    = [0 0 0];
positionCov = 0;
velocity    = rand(1,3);
velocityCov = 0;

predictDataSampleRate = 100;
fuseDataSampleRate = 2;
predictSamplesPerFuse = predictDataSampleRate/fuseDataSampleRate;

duration = 5;

for i = 1:duration*fuseDataSampleRate
    
    for j = 1:predictSamplesPerFuse
        
        predict(FUSE,accelData,gyroData);
        
    end
    
    fusegps(FUSE,position,positionCov,velocity,velocityCov);
    fusemag(FUSE,magData,magCov);
    
end

В любое время можно позвонить pose для возврата текущей оценки положения и ориентации:

[position, orientation] = pose(FUSE)

position = 1×3

10-15 ×
   -0.3331   -0.2775    0.3886

orientation = quaternion
      0.84705 -  0.25459i -  0.46613j - 0.020379k

Для полного примера рабочего процесса с использованием MARGGPSFuser, см. IMU и GPS Fusion для инерциальной навигации.

Плавкий акселерометр, гироскоп и GPS с неголономными ограничениями

Комбинированный акселерометр, гироскоп и данные GPS с неголономными ограничениями являются общей конфигурацией для оценки позы наземного транспортного средства. Чтобы сплавить акселерометр, гироскоп и данные GPS, создайте insfilterNonholonomic объект:

FUSE = insfilterNonholonomic

FUSE = 
  insfilterNonholonomic with properties:

        IMUSampleRate: 100        Hz         
    ReferenceLocation: [0 0 0]    [deg deg m]
     DecimationFactor: 2                     

   Extended Kalman Filter Values
              State: [16x1 double]     
    StateCovariance: [16x16 double]    

   Process Noise Variances
                  GyroscopeNoise: [4.8e-06 4.8e-06 4.8e-06]    (rad/s)²
              AccelerometerNoise: [0.048 0.048 0.048]          (m/s²)² 
              GyroscopeBiasNoise: [4e-14 4e-14 4e-14]          (rad/s)²
        GyroscopeBiasDecayFactor: 0.999                                
          AccelerometerBiasNoise: [4e-14 4e-14 4e-14]          (m/s²)² 
    AccelerometerBiasDecayFactor: 0.9999                               

   Measurement Noise Variances
    ZeroVelocityConstraintNoise: 0.01    (m/s)²

insfilterNonholonomic использует расширенный фильтр Калмана со следующими функциями:

predict - Обновление состояний с использованием данных акселерометра и гироскопа
fusegps - Правильные состояния с использованием данных GPS

Обычно данные акселерометра и гироскопа получают с более высокой скоростью, чем данные GPS. Для вызова можно использовать вложенные циклы predict и fusegps по разным ставкам.

accelData   = [0 0 9.8];
gyroData    = [0 0 0];
position    = [0 0 0];
positionCov = 0;
velocity    = rand(1,3);
velocityCov = 0;
predictDataSampleRate = 100;
fuseDataSampleRate = 2;
predictSamplesPerFuse = predictDataSampleRate/fuseDataSampleRate;
duration = 5;
for i = 1:duration*fuseDataSampleRate
    
    for j = 1:predictSamplesPerFuse
        
        predict(FUSE,accelData,gyroData);
        
    end
    
    fusegps(FUSE,position,positionCov,velocity,velocityCov);
    
end

В любое время можно позвонить pose для возврата текущей оценки положения и ориентации:

[position, orientation] = pose(FUSE)

position = 1×3

     0     0     0

orientation = quaternion

      0.9726 +       0i +       0j + 0.23248k

Для полного примера рабочего процесса с использованием NHConstrainedIMUGPSFuser, см. раздел Оценка положения и ориентации наземного транспортного средства.

См. также

Документация