Инерционная навигационная система и модель симуляции GNSS/GPS
The insSensor Система object™ моделирует устройство, которое запирает измерения от инерционной навигационной системы (INS) и глобальной навигационной спутниковой системы (GNSS), такой как GPS, и выводит сплавленные измерения.
Для вывода слитых измерений INS и GNSS:
Создайте insSensor Объекту и установите его свойства.
Вызывайте объект с аргументами, как будто это функция.
Дополнительные сведения о работе системных объектов см. в разделе «Что такое системные объекты?».
INS = insSensorINS, который моделирует устройство, которое выводит измерения из INS и GNSS.
measurement = INS(gTruth)gTruth.
measurement = INS(gTruth,simTime)simTime. Чтобы включить этот синтаксис, установите TimeInput свойство к true.
Чтобы использовать функцию объекта, задайте системный объект в качестве первого входного параметра. Например, чтобы освободить системные ресурсы системного объекта с именем obj, используйте следующий синтаксис:
release(obj)
Создайте структуру движения, которая задает стационарное положение на локальном северо-восточном нисходящем (NED) источнике. Поскольку платформа является стационарной, вам нужно задать только одну выборку. Предположим, что движение «земля-правда» дискретизируется в течение 10 секунд со скоростью дискретизации 100 Гц. Создайте insSensor по умолчанию Системные object™. Предварительно выделите переменные, чтобы сохранить выход из insSensor объект.
Fs = 100; duration = 10; numSamples = Fs*duration; motion = struct( ... 'Position',zeros(1,3), ... 'Velocity',zeros(1,3), ... 'Orientation',ones(1,1,'quaternion')); INS = insSensor; positionMeasurements = zeros(numSamples,3); velocityMeasurements = zeros(numSamples,3); orientationMeasurements = zeros(numSamples,1,'quaternion');
В цикле вызывайте INS со стационарной структурой движения для возврата измерений положения, скорости и ориентации в локальной системе координат NED. Логгирование измерений положения, скорости и ориентации.
for i = 1:numSamples measurements = INS(motion); positionMeasurements(i,:) = measurements.Position; velocityMeasurements(i,:) = measurements.Velocity; orientationMeasurements(i) = measurements.Orientation; end
Преобразуйте ориентацию из кватернионов в углы Эйлера в целях визуализации. Постройте график измерений положения, скорости и ориентации с течением времени.
orientationMeasurements = eulerd(orientationMeasurements,'ZYX','frame'); t = (0:(numSamples-1))/Fs; subplot(3,1,1) plot(t,positionMeasurements) title('Position') xlabel('Time (s)') ylabel('Position (m)') legend('North','East','Down') subplot(3,1,2) plot(t,velocityMeasurements) title('Velocity') xlabel('Time (s)') ylabel('Velocity (m/s)') legend('North','East','Down') subplot(3,1,3) plot(t,orientationMeasurements) title('Orientation') xlabel('Time (s)') ylabel('Rotation (degrees)') legend('Roll', 'Pitch', 'Yaw')
Сгенерируйте измерения INS с помощью insSensor Системные object™. Использование waypointTrajectory чтобы сгенерировать путь «основная истина». Использование trackingScenario организовать симуляцию и визуализировать движение.
Задайте траекторию основной истины как восьмерку пути в плоскости Северо-Восток. Используйте частоту дискретизации 50 Гц и длительность 5 секунд.
Fs = 50; duration = 5; numSamples = Fs*duration; t = (0:(numSamples-1)).'/Fs; a = 2; x = a.*sqrt(2).*cos(t) ./ (sin(t).^2 + 1); y = sin(t) .* x; z = zeros(numSamples,1); waypoints = [x,y,z]; path = waypointTrajectory('Waypoints',waypoints,'TimeOfArrival',t);
Создайте insSensor Системный объект для модели приема данных INS. Установите PositionAccuracy на 0.1.
ins = insSensor('PositionAccuracy',0.1);Создайте сценарий отслеживания с одной платформой, движение которой определяется path.
scenario = trackingScenario('UpdateRate',Fs);
quadcopter = platform(scenario);
quadcopter.Trajectory = path;Создайте театральный график, чтобы визуализировать основную истину quadcopter движение и квадрокоптерные измерения движения, смоделированные insSensor.
tp = theaterPlot('XLimits',[-3, 3],'YLimits', [-3, 3]); quadPlotter = platformPlotter(tp, ... 'DisplayName', 'Ground-Truth Motion', ... 'Marker', 's', ... 'MarkerFaceColor','blue'); insPlotter = detectionPlotter(tp, ... 'DisplayName','INS Measurement', ... 'Marker','d', ... 'MarkerFaceColor','red');
В цикле продвигайте сценарий до его завершения. Для каждого временного шага получите текущую выборку движения, смоделируйте измерения INS для движения и затем постройте график результата.
while advance(scenario) motion = platformPoses(scenario,'quaternion'); insMeas = ins(motion); plotPlatform(quadPlotter,motion.Position); plotDetection(insPlotter,insMeas.Position); pause(1/scenario.UpdateRate) end
Сгенерируйте измерения INS с помощью insSensor Системные object™. Использование waypointTrajectory чтобы сгенерировать путь «основная истина».
Задайте ориентацию «земля-правда», которая начинается с оси X тела датчика, выровненной по северу, и заканчивается осью X тела датчика, выровненной по Востоку. Задайте точки пути для дуговой траектории и вектор времени прибытия для соответствующих точек пути. Используйте частоту дискретизации 100 Гц. Создайте waypointTrajectory Системный объект с ограничениями путевой точки и установите SamplesPerFrame так что вся траектория выводится одним вызовом.
eulerAngles = [0,0,0; ... 0,0,0; ... 90,0,0; ... 90,0,0]; orientation = quaternion(eulerAngles,'eulerd','ZYX','frame'); r = 20; waypoints = [0,0,0; ... 100,0,0; ... 100+r,r,0; ... 100+r,100+r,0]; toa = [0,10,10+(2*pi*r/4),20+(2*pi*r/4)]; Fs = 100; numSamples = floor(Fs*toa(end)); path = waypointTrajectory('Waypoints',waypoints, ... 'TimeOfArrival',toa, ... 'Orientation',orientation, ... 'SampleRate',Fs, ... 'SamplesPerFrame',numSamples);
Создайте insSensor Системный объект для модели приема данных INS. Установите PositionAccuracy на 0.1.
ins = insSensor('PositionAccuracy',0.1);Вызовите объект траектории путевой точки, path, чтобы сгенерировать движение « основную истину». Вызовите симулятор INS, ins, с движением основная истина, чтобы сгенерировать измерения INS.
[motion.Position,motion.Orientation,motion.Velocity] = path(); insMeas = ins(motion);
Преобразуйте ориентацию, возвращенную ins для углов Эйлера в степенях в целях визуализации. Постройте график полного пути и ориентации с течением времени.
orientationMeasurementEuler = eulerd(insMeas.Orientation,'ZYX','frame'); subplot(2,1,1) plot(insMeas.Position(:,1),insMeas.Position(:,2)); title('Path') xlabel('North (m)') ylabel('East (m)') subplot(2,1,2) t = (0:(numSamples-1)).'/Fs; plot(t,orientationMeasurementEuler(:,1), ... t,orientationMeasurementEuler(:,2), ... t,orientationMeasurementEuler(:,3)); title('Orientation') legend('Yaw','Pitch','Roll') xlabel('Time (s)') ylabel('Rotation (degrees)')
