Инерционное состояние основной истины»
gTruth = state(ac)insSensor Системные object™.
Эта функция поддерживается только для актёров, у которых есть траектории, созданные при помощи smoothTrajectory функция.
Сгенерируйте измерения с датчика INS, который установлен на транспортном средстве в сценарии вождения. Постройте график измерений INS в зависимости от состояния основной истины транспортного средства и визуализируйте скорость и профиль ускорения транспортного средства.
Создайте сценарий вождения
Загрузите географические данные для маршрута в кампусе MathWorks Apple Hill в Натике, штат Массачусетс.
data = load('ahroute.mat');
latIn = data.latitude;
lonIn = data.longitude;
Преобразуйте координаты широты и долготы трассы в Декартовы координаты. Установите источник первой координаты в ведущем маршруте. Для простоты примите высоту 0 для маршрута.
alt = 0; origin = [latIn(1),lonIn(1),alt]; [xEast,yNorth,zUp] = latlon2local(latIn,lonIn,alt,origin);
Создайте сценарий вождения. Установите источник преобразованной трассы в качестве географической опорной точки.
scenario = drivingScenario('GeoReference',origin);
Создайте дорогу на основе Декартовых координат трассы.
roadCenters = [xEast,yNorth,zUp]; road(scenario,roadCenters);
Создайте транспортное средство, которое следует по осевой линии дороги. Транспортное средство перемещается между 4 и 5 метрами в секунду (от 9 до 11 миль в час), замедляясь на кривых в дороге. Чтобы создать траекторию, используйте smoothTrajectory функция. Вычисленная траектория минимизирует рывок и избегает разрывов в ускорении, что является требованием для моделирования датчиков INS.
egoVehicle = vehicle(scenario,'ClassID',1);
egoPath = roadCenters;
egoSpeed = [5 5 5 4 4 4 5 4 4 4 4 5 5 5 5 5];
smoothTrajectory(egoVehicle,egoPath,egoSpeed);
Постройте график сценария и отобразите вид 3-D из-за автомобиля , оборудованного датчиком.
plot(scenario) chasePlot(egoVehicle)
Создайте датчик INS
Создайте датчик INS, который принимает вход времен симуляции. Вводите шум в измерения датчика путем установки стандартного отклонения скорости и точности измерений 0,1 и 0,05 соответственно.
INS = insSensor('TimeInput',true, ... 'VelocityAccuracy',0.1, ... 'AccelerationAccuracy',0.05);
Визуализация измерений INS
Инициализируйте географический проигрыватель для отображения измерений INS и основной истины актёра. Сконфигурируйте проигрыватель, чтобы отобразить последние 10 позиций и установить уровень масштаба равным 17.
zoomLevel = 17; player = geoplayer(latIn(1),lonIn(1),zoomLevel, ... 'HistoryDepth',10,'HistoryStyle','line');
Предварительно выделите пространство для времен симуляции, измерений скорости и измерений ускорения, которые захватываются во время симуляции.
numWaypoints = length(latIn); times = zeros(numWaypoints,1); gTruthVelocities = zeros(numWaypoints,1); gTruthAccelerations = zeros(numWaypoints,1); sensorVelocities = zeros(numWaypoints,1); sensorAccelerations = zeros(numWaypoints,1);
Симулируйте сценарий. Во время цикла симуляции получите основная истина состояние автомобиля , оборудованного датчиком и измерение INS этого состояния. Преобразуйте эти показания в географические координаты и в каждой путевой точке визуализируйте показания основной истины и INS на географическом проигрывателе. Также соберите данные о скорости и ускорении для графического изображения графиков скорости и ускорения.
nextWaypoint = 2; while advance(scenario) % Obtain ground truth state of ego vehicle. gTruth = state(egoVehicle); % Obtain INS sensor measurement. measurement = INS(gTruth,scenario.SimulationTime); % Convert readings to geographic coordinates. [latOut,lonOut] = local2latlon(measurement.Position(1), ... measurement.Position(2), ... measurement.Position(3),origin); % Plot differences between ground truth locations and locations reported by sensor. reachedWaypoint = sum(abs(roadCenters(nextWaypoint,:) - gTruth.Position)) < 1; if reachedWaypoint plotPosition(player,latIn(nextWaypoint),lonIn(nextWaypoint),'TrackID',1) plotPosition(player,latOut,lonOut,'TrackID',2,'Label','INS') % Capture simulation times, velocities, and accelerations. times(nextWaypoint,1) = scenario.SimulationTime; gTruthVelocities(nextWaypoint,1) = gTruth.Velocity(2); gTruthAccelerations(nextWaypoint,1) = gTruth.Acceleration(2); sensorVelocities(nextWaypoint,1) = measurement.Velocity(2); sensorAccelerations(nextWaypoint,1) = measurement.Acceleration(2); nextWaypoint = nextWaypoint + 1; end if nextWaypoint > numWaypoints break end end
Графический профиль скорости
Сравните продольную скорость транспортного средства по основной истине с измерениями скорости, захваченными датчиком INS.
Удалите нули из временных векторов и векторов скорости.
times(times == 0) = []; gTruthVelocities(gTruthVelocities == 0) = []; sensorVelocities(sensorVelocities == 0) = []; figure hold on plot(times,gTruthVelocities) plot(times,sensorVelocities) title('Longitudinal Velocity Profile') xlabel('Time (s)') ylabel('Velocity (m/s)') legend('Ground truth','INS') hold off
Графический профиль ускорения графика
Сравните основную истину продольное ускорение транспортного средства по времени с измерениями ускорения, захваченными датчиком INS.
gTruthAccelerations(gTruthAccelerations == 0) = []; sensorAccelerations(sensorAccelerations == 0) = []; figure hold on plot(times,gTruthAccelerations) plot(times,sensorAccelerations) title('Longitudinal Acceleration Profile') xlabel('Time (s)') ylabel('Acceleration (m/s^2)') legend('Ground truth','INS') hold off
actor | drivingScenario | insSensor | smoothTrajectory | vehicle