Определите положение Используя инерционные датчики и GPS

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

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

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

Чтобы изучить, как смоделировать инерционные датчики и GPS, смотрите IMU Модели, GPS и INS/GPS. Чтобы изучить, как сгенерировать движение основной истины что модели датчика дисков, смотрите waypointTrajectory и kinematicTrajectory.

Можно также объединить инерционные данные о датчике без GPS, чтобы оценить ориентацию. Смотрите Определяют Ориентацию Используя Инерционные Датчики.

Плавьте Инерционный Датчик и данные о GPS

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

Плавьте акселерометр, гироскоп и 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, смотрите Оценочное Положение и Ориентацию Наземного транспортного средства.

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