Настройка insfilterNonholonomic
параметры для уменьшения ошибки расчета
настраивает свойства tunedMeasureNoise
= tune(filter
,measureNoise
,sensorData
,groundTruth
)insfilterNonholonomic
объект фильтра, filter
и измерительные шумы для уменьшения среднеквадратичного состояния (RMS) ошибки расчета между данными сплавленного датчика и основной истины. Функция также возвращает настроенный шум измерения tunedMeasureNoise
. Функция использует значения свойств в фильтре и шум измерения, предоставленный в measureNoise
структура как начальная оценка для алгоритма оптимизации.
задает строение на основе tunedMeasureNoise
= tune(___,config
)tunerconfig
объект, config
.
insfilterNonholonomic
Оптимизация оценки положенияЗагрузите записанные данные датчика и достоверные данные.
load('insfilterNonholonomicTuneData.mat');
Создайте таблицы для данных датчика и данных истинности.
sensorData = table(Accelerometer, Gyroscope, ...
GPSPosition, GPSVelocity);
groundTruth = table(Orientation, Position);
Создайте insfilterNonholonimic
объект фильтра.
filter = insfilterNonholonomic('State', initialState, ... 'StateCovariance', initialStateCovariance, ... 'DecimationFactor', 1);
Создайте объект строения тюнера для фильтра. Установите максимальное количество итераций равным 30.
config = tunerconfig('insfilterNonholonomic','MaxIterations',30);
Используйте tunernoise
функция для получения набора начальных шумов датчика, используемых в фильтре.
measNoise = tunernoise('insfilterNonholonomic')
measNoise = struct with fields:
GPSPositionNoise: 1
GPSVelocityNoise: 1
Настройте фильтр и получите настроенный шум измерения.
tunedNoise = tune(filter,measNoise,sensorData,groundTruth,config);
Iteration Parameter Metric _________ _________ ______ 1 GyroscopeNoise 3.4877 1 AccelerometerNoise 3.3961 1 GyroscopeBiasNoise 3.3961 1 GyroscopeBiasDecayFactor 3.3961 1 AccelerometerBiasNoise 3.3961 1 AccelerometerBiasDecayFactor 3.3961 1 ZeroVelocityConstraintNoise 3.3935 1 GPSPositionNoise 3.2848 1 GPSVelocityNoise 3.2798 2 GyroscopeNoise 3.2641 2 AccelerometerNoise 3.1715 2 GyroscopeBiasNoise 3.1715 2 GyroscopeBiasDecayFactor 2.9661 2 AccelerometerBiasNoise 2.9661 2 AccelerometerBiasDecayFactor 2.9661 2 ZeroVelocityConstraintNoise 2.9617 2 GPSPositionNoise 2.8438 2 GPSVelocityNoise 2.8384 3 GyroscopeNoise 2.8373 3 AccelerometerNoise 2.7382 3 GyroscopeBiasNoise 2.7382 3 GyroscopeBiasDecayFactor 2.7382 3 AccelerometerBiasNoise 2.7382 3 AccelerometerBiasDecayFactor 2.7382 3 ZeroVelocityConstraintNoise 2.7335 3 GPSPositionNoise 2.6105 3 GPSVelocityNoise 2.6045 4 GyroscopeNoise 2.6023 4 AccelerometerNoise 2.5001 4 GyroscopeBiasNoise 2.5001 4 GyroscopeBiasDecayFactor 2.5001 4 AccelerometerBiasNoise 2.5001 4 AccelerometerBiasDecayFactor 2.5001 4 ZeroVelocityConstraintNoise 2.4953 4 GPSPositionNoise 2.3692 4 GPSVelocityNoise 2.3626 5 GyroscopeNoise 2.3595 5 AccelerometerNoise 2.2561 5 GyroscopeBiasNoise 2.2561 5 GyroscopeBiasDecayFactor 2.2508 5 AccelerometerBiasNoise 2.2508 5 AccelerometerBiasDecayFactor 2.2508 5 ZeroVelocityConstraintNoise 2.2469 5 GPSPositionNoise 2.1265 5 GPSVelocityNoise 2.1191 6 GyroscopeNoise 2.1148 6 AccelerometerNoise 2.0150 6 GyroscopeBiasNoise 2.0150 6 GyroscopeBiasDecayFactor 2.0150 6 AccelerometerBiasNoise 2.0150 6 AccelerometerBiasDecayFactor 2.0150 6 ZeroVelocityConstraintNoise 2.0116 6 GPSPositionNoise 1.8970 6 GPSVelocityNoise 1.8888 7 GyroscopeNoise 1.8847 7 AccelerometerNoise 1.7921 7 GyroscopeBiasNoise 1.7921 7 GyroscopeBiasDecayFactor 1.7845 7 AccelerometerBiasNoise 1.7845 7 AccelerometerBiasDecayFactor 1.7845 7 ZeroVelocityConstraintNoise 1.7815 7 GPSPositionNoise 1.6794 7 GPSVelocityNoise 1.6708 8 GyroscopeNoise 1.6679 8 AccelerometerNoise 1.5886 8 GyroscopeBiasNoise 1.5886 8 GyroscopeBiasDecayFactor 1.5866 8 AccelerometerBiasNoise 1.5866 8 AccelerometerBiasDecayFactor 1.5866 8 ZeroVelocityConstraintNoise 1.5850 8 GPSPositionNoise 1.5057 8 GPSVelocityNoise 1.4965 9 GyroscopeNoise 1.4950 9 AccelerometerNoise 1.4364 9 GyroscopeBiasNoise 1.4364 9 GyroscopeBiasDecayFactor 1.4364 9 AccelerometerBiasNoise 1.4364 9 AccelerometerBiasDecayFactor 1.4364 9 ZeroVelocityConstraintNoise 1.4355 9 GPSPositionNoise 1.3894 9 GPSVelocityNoise 1.3790 10 GyroscopeNoise 1.3773 10 AccelerometerNoise 1.3422 10 GyroscopeBiasNoise 1.3422 10 GyroscopeBiasDecayFactor 1.3421 10 AccelerometerBiasNoise 1.3421 10 AccelerometerBiasDecayFactor 1.3421 10 ZeroVelocityConstraintNoise 1.3399 10 GPSPositionNoise 1.3319 10 GPSVelocityNoise 1.3190 11 GyroscopeNoise 1.3159 11 AccelerometerNoise 1.3102 11 GyroscopeBiasNoise 1.3102 11 GyroscopeBiasDecayFactor 1.3100 11 AccelerometerBiasNoise 1.3100 11 AccelerometerBiasDecayFactor 1.3100 11 ZeroVelocityConstraintNoise 1.3069 11 GPSPositionNoise 1.2964 11 GPSVelocityNoise 1.2762 12 GyroscopeNoise 1.2740 12 AccelerometerNoise 1.2655 12 GyroscopeBiasNoise 1.2655 12 GyroscopeBiasDecayFactor 1.2641 12 AccelerometerBiasNoise 1.2641 12 AccelerometerBiasDecayFactor 1.2641 12 ZeroVelocityConstraintNoise 1.2631 12 GPSPositionNoise 1.2511 12 GPSVelocityNoise 1.2198 13 GyroscopeNoise 1.2184 13 AccelerometerNoise 1.2058 13 GyroscopeBiasNoise 1.2058 13 GyroscopeBiasDecayFactor 1.2029 13 AccelerometerBiasNoise 1.2029 13 AccelerometerBiasDecayFactor 1.2029 13 ZeroVelocityConstraintNoise 1.2029 13 GPSPositionNoise 1.1874 13 GPSVelocityNoise 1.1408 14 GyroscopeNoise 1.1403 14 AccelerometerNoise 1.1236 14 GyroscopeBiasNoise 1.1236 14 GyroscopeBiasDecayFactor 1.1186 14 AccelerometerBiasNoise 1.1186 14 AccelerometerBiasDecayFactor 1.1186 14 ZeroVelocityConstraintNoise 1.1183 14 GPSPositionNoise 1.0975 14 GPSVelocityNoise 1.0348 15 GyroscopeNoise 1.0347 15 AccelerometerNoise 1.0155 15 GyroscopeBiasNoise 1.0155 15 GyroscopeBiasDecayFactor 1.0081 15 AccelerometerBiasNoise 1.0081 15 AccelerometerBiasDecayFactor 1.0081 15 ZeroVelocityConstraintNoise 1.0076 15 GPSPositionNoise 0.9813 15 GPSVelocityNoise 0.9078 16 GyroscopeNoise 0.9074 16 AccelerometerNoise 0.8926 16 GyroscopeBiasNoise 0.8926 16 GyroscopeBiasDecayFactor 0.8823 16 AccelerometerBiasNoise 0.8823 16 AccelerometerBiasDecayFactor 0.8823 16 ZeroVelocityConstraintNoise 0.8815 16 GPSPositionNoise 0.8526 16 GPSVelocityNoise 0.7926 17 GyroscopeNoise 0.7920 17 AccelerometerNoise 0.7870 17 GyroscopeBiasNoise 0.7870 17 GyroscopeBiasDecayFactor 0.7742 17 AccelerometerBiasNoise 0.7742 17 AccelerometerBiasDecayFactor 0.7742 17 ZeroVelocityConstraintNoise 0.7730 17 GPSPositionNoise 0.7665 17 GPSVelocityNoise 0.7665 18 GyroscopeNoise 0.7662 18 AccelerometerNoise 0.7638 18 GyroscopeBiasNoise 0.7638 18 GyroscopeBiasDecayFactor 0.7495 18 AccelerometerBiasNoise 0.7495 18 AccelerometerBiasDecayFactor 0.7495 18 ZeroVelocityConstraintNoise 0.7482 18 GPSPositionNoise 0.7482 18 GPSVelocityNoise 0.7475 19 GyroscopeNoise 0.7474 19 AccelerometerNoise 0.7474 19 GyroscopeBiasNoise 0.7474 19 GyroscopeBiasDecayFactor 0.7474 19 AccelerometerBiasNoise 0.7474 19 AccelerometerBiasDecayFactor 0.7474 19 ZeroVelocityConstraintNoise 0.7453 19 GPSPositionNoise 0.7416 19 GPSVelocityNoise 0.7382 20 GyroscopeNoise 0.7378 20 AccelerometerNoise 0.7370 20 GyroscopeBiasNoise 0.7370 20 GyroscopeBiasDecayFactor 0.7370 20 AccelerometerBiasNoise 0.7370 20 AccelerometerBiasDecayFactor 0.7370 20 ZeroVelocityConstraintNoise 0.7345 20 GPSPositionNoise 0.7345 20 GPSVelocityNoise 0.7345 21 GyroscopeNoise 0.7334 21 AccelerometerNoise 0.7334 21 GyroscopeBiasNoise 0.7334 21 GyroscopeBiasDecayFactor 0.7334 21 AccelerometerBiasNoise 0.7334 21 AccelerometerBiasDecayFactor 0.7334 21 ZeroVelocityConstraintNoise 0.7306 21 GPSPositionNoise 0.7279 21 GPSVelocityNoise 0.7268 22 GyroscopeNoise 0.7248 22 AccelerometerNoise 0.7247 22 GyroscopeBiasNoise 0.7247 22 GyroscopeBiasDecayFactor 0.7234 22 AccelerometerBiasNoise 0.7234 22 AccelerometerBiasDecayFactor 0.7234 22 ZeroVelocityConstraintNoise 0.7207 22 GPSPositionNoise 0.7206 22 GPSVelocityNoise 0.7170 23 GyroscopeNoise 0.7138 23 AccelerometerNoise 0.7134 23 GyroscopeBiasNoise 0.7134 23 GyroscopeBiasDecayFactor 0.7134 23 AccelerometerBiasNoise 0.7134 23 AccelerometerBiasDecayFactor 0.7134 23 ZeroVelocityConstraintNoise 0.7122 23 GPSPositionNoise 0.7122 23 GPSVelocityNoise 0.7122 24 GyroscopeNoise 0.7081 24 AccelerometerNoise 0.7080 24 GyroscopeBiasNoise 0.7080 24 GyroscopeBiasDecayFactor 0.7080 24 AccelerometerBiasNoise 0.7080 24 AccelerometerBiasDecayFactor 0.7080 24 ZeroVelocityConstraintNoise 0.7080 24 GPSPositionNoise 0.7080 24 GPSVelocityNoise 0.7072 25 GyroscopeNoise 0.7009 25 AccelerometerNoise 0.7009 25 GyroscopeBiasNoise 0.7009 25 GyroscopeBiasDecayFactor 0.7007 25 AccelerometerBiasNoise 0.7007 25 AccelerometerBiasDecayFactor 0.7007 25 ZeroVelocityConstraintNoise 0.7005 25 GPSPositionNoise 0.6997 25 GPSVelocityNoise 0.6997 26 GyroscopeNoise 0.6912 26 AccelerometerNoise 0.6906 26 GyroscopeBiasNoise 0.6906 26 GyroscopeBiasDecayFactor 0.6906 26 AccelerometerBiasNoise 0.6906 26 AccelerometerBiasDecayFactor 0.6906 26 ZeroVelocityConstraintNoise 0.6896 26 GPSPositionNoise 0.6896 26 GPSVelocityNoise 0.6896 27 GyroscopeNoise 0.6840 27 AccelerometerNoise 0.6831 27 GyroscopeBiasNoise 0.6831 27 GyroscopeBiasDecayFactor 0.6831 27 AccelerometerBiasNoise 0.6831 27 AccelerometerBiasDecayFactor 0.6831 27 ZeroVelocityConstraintNoise 0.6818 27 GPSPositionNoise 0.6816 27 GPSVelocityNoise 0.6816 28 GyroscopeNoise 0.6816 28 AccelerometerNoise 0.6809 28 GyroscopeBiasNoise 0.6809 28 GyroscopeBiasDecayFactor 0.6809 28 AccelerometerBiasNoise 0.6809 28 AccelerometerBiasDecayFactor 0.6809 28 ZeroVelocityConstraintNoise 0.6804 28 GPSPositionNoise 0.6802 28 GPSVelocityNoise 0.6802 29 GyroscopeNoise 0.6793 29 AccelerometerNoise 0.6785 29 GyroscopeBiasNoise 0.6785 29 GyroscopeBiasDecayFactor 0.6785 29 AccelerometerBiasNoise 0.6785 29 AccelerometerBiasDecayFactor 0.6785 29 ZeroVelocityConstraintNoise 0.6778 29 GPSPositionNoise 0.6773 29 GPSVelocityNoise 0.6773 30 GyroscopeNoise 0.6773 30 AccelerometerNoise 0.6769 30 GyroscopeBiasNoise 0.6769 30 GyroscopeBiasDecayFactor 0.6769 30 AccelerometerBiasNoise 0.6769 30 AccelerometerBiasDecayFactor 0.6769 30 ZeroVelocityConstraintNoise 0.6769 30 GPSPositionNoise 0.6769 30 GPSVelocityNoise 0.6769
Предохраните данные датчика с помощью настроенного фильтра. Получите расчетное положение и ориентацию.
N = size(sensorData,1); qEstTuned = quaternion.zeros(N,1); posEstTuned = zeros(N,3); for ii=1:N predict(filter,Accelerometer(ii,:),Gyroscope(ii,:)); if all(~isnan(GPSPosition(ii,1))) fusegps(filter, GPSPosition(ii,:), ... tunedNoise.GPSPositionNoise,GPSVelocity(ii,:), ... tunedNoise.GPSVelocityNoise); end [posEstTuned(ii,:),qEstTuned(ii,:)] = pose(filter); end
Вычислите ошибки RMS.
orientationErrorTuned = rad2deg(dist(qEstTuned,Orientation)); rmsOrientationErrorTuned = sqrt(mean(orientationErrorTuned.^2))
rmsOrientationErrorTuned = 1.6857
positionErrorTuned = sqrt(sum((posEstTuned-Position).^2,2)); rmsPositionErrorTuned = sqrt(mean(positionErrorTuned.^2))
rmsPositionErrorTuned = 1.6667
Визуализация результатов.
figure; t = (0:N-1)./filter.IMUSampleRate; subplot(2,1,1) plot(t,positionErrorTuned,'b'); title("Tuned insfilterNonholonomic" + newline + ... "Euclidean Distance Position Error") xlabel('Time (s)'); ylabel('Position Error (meters)') subplot(2,1,2) plot(t,orientationErrorTuned,'b'); title("Orientation Error") xlabel('Time (s)'); ylabel('Orientation Error (degrees)');
filter
- Объект фильтраinfilterAsync
объектОбъект фильтра, заданный как insfilterNonholonomic
объект.
measureNoise
- Измерительный шумИзмерительный шум, заданный как структура. Функция использует вход шума измерения в качестве начального предположения для настройки шума измерения. Структура должна содержать следующие поля:
Имя поля | Описание |
---|---|
GPSPositionNoise | Отклонение шума положения GPS, заданная как скаляр в m2 |
GPSVelocityNoise | Отклонение шума скорости GPS, заданная как скаляр в (м/с)2 |
Типы данных: struct
sensorData
- Данные о датчикеtable
Данные о датчике, заданные как table
. В каждой строке данные о датчике задаются как:
Accelerometer
- Данные акселерометра, заданные как вектор скаляров 1 на 3 в m2/ с .
Gyroscope
- Данные гироскопа, заданные как вектор 1 на 3 скаляров в рад/с.
GPSPosition
- данные положения GPS, заданные как вектор 1 на 3 скаляров в метрах.
GPSVelocity
- данные скорости GPS, заданные как вектор 1 на 3 скаляров в м/с.
Если GPS-датчик не производит полных измерений, задайте соответствующую запись для GPSPosition
и/или GPSVelocity
как NaN
. Если вы задаете Cost
свойство входа строения тюнера, config
, в Custom
, затем можно использовать другие типы данных для sensorData
вход, основанный на вашем выборе.
Типы данных: table
groundTruth
- Достоверные данныеtable
Достоверные данные, заданные как таблица. В каждой строке таблица может опционально содержать любую из следующих переменных:
Orientation
- Ориентация от навигационной системы координат к каркасу кузова, заданная как quaternion
или матрица вращения 3 на 3.
Position
- Положение в навигационной системе координат, заданное как вектор 1 на 3 скаляров в метрах.
Velocity
- Скорость в навигационной системе координат, заданная как вектор 1 на 3 скаляров в м/с.
GyroscopeBias
- Смещение угла дельты гироскопа в каркасе кузова, заданное как вектор 1 на 3 скаляров в рад/с.
AccelerometerBias
- Смещение угла дельты акселерометра в каркасе кузова, заданное как вектор скаляров 1 на 3 в m2/ с .
Функция обрабатывает каждую строку sensorData
и groundTruth
таблицы для вычисления оценки состояния и ошибки RMS из основной истины. Переменные состояния, отсутствующие в groundTruth
входы для сравнения игнорируются. The sensorData
и groundTruth
таблицы должны иметь одинаковое число строк.
Если вы задаете Cost
свойство входа строения тюнера, config
, в Custom
, затем можно использовать другие типы данных для groundTruth
вход, основанный на вашем выборе.
Типы данных: table
config
- Настройка тюнераtunerconfig
объектСтроение тюнера, заданный как tunerconfig
объект.
tunedMeasureNoise
- Настроенный шум измеренияНастроенный шум измерения, возвращенный как структура. Структура содержит эти поля.
Имя поля | Описание |
---|---|
GPSPositionNoise | Отклонение шума положения GPS, заданная как скаляр в m2 |
GPSVelocityNoise | Отклонение шума скорости GPS, заданная как скаляр в (м/с)2 |
Типы данных: struct
[1] Abbeel, Pieter, et al. Дискриминационное обучение фильтров Калмана. Robotics: Science and Systems I, Robotics: Science and Systems Foundation, 2005. DOI.org (Crossref), doi:10.15607/RSS.2005.I.038.
У вас есть измененная версия этого примера. Вы хотите открыть этот пример с вашими правками?
1. Если смысл перевода понятен, то лучше оставьте как есть и не придирайтесь к словам, синонимам и тому подобному. О вкусах не спорим.
2. Не дополняйте перевод комментариями “от себя”. В исправлении не должно появляться дополнительных смыслов и комментариев, отсутствующих в оригинале. Такие правки не получится интегрировать в алгоритме автоматического перевода.
3. Сохраняйте структуру оригинального текста - например, не разбивайте одно предложение на два.
4. Не имеет смысла однотипное исправление перевода какого-то термина во всех предложениях. Исправляйте только в одном месте. Когда Вашу правку одобрят, это исправление будет алгоритмически распространено и на другие части документации.
5. По иным вопросам, например если надо исправить заблокированное для перевода слово, обратитесь к редакторам через форму технической поддержки.