Определите положение Используя инерционные датчики и 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, создайте объект MARGGPSFuser с помощью функции insfilter:

FUSE = insfilter('NonholonomicHeading',false,'Magnetometer',true)

FUSE = 

  MARGGPSFuser 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²

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

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*predictDataSampleRate
    
    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.0e-15 *

   -0.0000   -0.0555    0.1110


orientation = 

  quaternion


     0.65342 + 0.56507i + 0.31113j + 0.39615k

Для полного рабочего процесса в качестве примера с помощью MARGGPSFuser см. IMU и Fusion GPS для Инерционной Навигации.

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

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

FUSE = insfilter('NonholonomicHeading',true,'Magnetometer',false)

FUSE = 

  NHConstrainedIMUGPSFuser 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)²

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

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*predictDataSampleRate
    
    for j = 1:predictSamplesPerFuse
        
        predict(FUSE,accelData,gyroData);
        
    end
    
    fusegps(FUSE,position,positionCov,velocity,velocityCov);
    
end

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

[position, orientation] = pose(FUSE)

position =

   1.0e-15 *

   -0.0000   -0.0555    0.1110


orientation = 

  quaternion


     0.65342 + 0.56507i + 0.31113j + 0.39615k

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

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