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

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

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

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

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

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

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

Инерционная навигационная система (INS) состоит из датчиков, которые обнаруживают поступательное движение (акселерометры), вращение (гироскопы), и в некоторых системах магнитные поля (магнитометры). При непрерывном сплавлении данных о датчике INS может считаться с положением платформы без внешних датчиков. Однако оценка положения 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 для инерционной навигации.

Предохранитель Accelerometer, гироскоп и 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, см. «Оценка положения и ориентации наземного транспортного средства».

См. также

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