Инерционная навигация и имитационная модель GPS
insSensor Система object™ вывод данных моделей от инерционной навигации и GPS.
К выходу модели от инерционной навигации и GPS:
Создайте insSensor объект и набор его свойства.
Вызовите объект с аргументами, как будто это была функция.
Чтобы узнать больше, как Системные объекты работают, смотрите то, Что Системные объекты?.
measurement = INS(motion)motion.
Чтобы использовать объектную функцию, задайте Системный объект как первый входной параметр. Например, чтобы выпустить системные ресурсы Системного объекта под названием obj, используйте этот синтаксис:
release(obj)
Создайте struct движения, который задает стационарное положение в локальном начале координат 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 со стационарным struct движения, чтобы возвратить положение, скорость и измерения ориентации в локальной системе координат 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 движение и 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 корпуса датчика, выровненной с Востоком. Задайте waypoints для траектории дуги и вектор времени прибытия для соответствующего waypoints. Используйте частоту дискретизации на 100 Гц. Создайте waypointTrajectory Системный объект с waypoint ограничениями и набор 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);Вызовите waypoint объект траектории, 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)')
