Настройка insfilterErrorState
параметры для уменьшения ошибки расчета
настраивает свойства tunedMeasureNoise
= tune(filter
,measureNoise
,sensorData
,groundTruth
)insfilterErrorState
объект фильтра, filter
и измерительные шумы для уменьшения среднеквадратичного состояния (RMS) ошибки расчета между данными сплавленного датчика и основной истины. Функция также возвращает настроенный шум измерения tunedMeasureNoise
. Функция использует значения свойств в фильтре и шум измерения, предоставленный в measureNoise
структура как начальная оценка для алгоритма оптимизации.
задает строение на основе tunedMeasureNoise
= tune(___,config
)tunerconfig
объект, config
.
insfilterErrorState
Оптимизация оценки положенияЗагрузите записанные данные датчика и достоверные данные.
load('insfilterErrorStateTuneData.mat');
Создайте таблицы для данных датчика и данных истинности.
sensorData = table(Accelerometer,Gyroscope, ... GPSPosition,GPSVelocity,MVOOrientation, ... MVOPosition); groundTruth = table(Orientation,Position);
Создайте insfilterErrorState
объект фильтра.
filter = insfilterErrorState('State',initialState, ... 'StateCovariance',initialStateCovariance);
Создайте объект строения тюнера для фильтра. Используйте функцию шума тюнера, чтобы получить набор начальных шумов датчика, используемых в фильтре.
cfg = tunerconfig('insfilterErrorState','MaxIterations',40); measNoise = tunernoise('insfilterErrorState')
measNoise = struct with fields:
MVOOrientationNoise: 1
MVOPositionNoise: 1
GPSPositionNoise: 1
GPSVelocityNoise: 1
Настройте фильтр и получите настроенные параметры.
tunedmn = tune(filter,measNoise,sensorData, ...
groundTruth,cfg);
Iteration Parameter Metric _________ _________ ______ 1 AccelerometerNoise 4.1291 1 GyroscopeNoise 4.1291 1 AccelerometerBiasNoise 4.1290 1 GyroscopeBiasNoise 4.1290 1 GPSPositionNoise 4.0213 1 GPSVelocityNoise 4.0051 1 MVOPositionNoise 3.9949 1 MVOOrientationNoise 3.9886 2 AccelerometerNoise 3.9886 2 GyroscopeNoise 3.9886 2 AccelerometerBiasNoise 3.9886 2 GyroscopeBiasNoise 3.9886 2 GPSPositionNoise 3.8381 2 GPSVelocityNoise 3.8268 2 MVOPositionNoise 3.8219 2 MVOOrientationNoise 3.8035 3 AccelerometerNoise 3.8035 3 GyroscopeNoise 3.8035 3 AccelerometerBiasNoise 3.8035 3 GyroscopeBiasNoise 3.8035 3 GPSPositionNoise 3.6299 3 GPSVelocityNoise 3.6276 3 MVOPositionNoise 3.6241 3 MVOOrientationNoise 3.5911 4 AccelerometerNoise 3.5911 4 GyroscopeNoise 3.5911 4 AccelerometerBiasNoise 3.5911 4 GyroscopeBiasNoise 3.5911 4 GPSPositionNoise 3.1728 4 GPSVelocityNoise 3.1401 4 MVOPositionNoise 2.7686 4 MVOOrientationNoise 2.6632 5 AccelerometerNoise 2.6632 5 GyroscopeNoise 2.6632 5 AccelerometerBiasNoise 2.6632 5 GyroscopeBiasNoise 2.6632 5 GPSPositionNoise 2.3242 5 GPSVelocityNoise 2.2291 5 MVOPositionNoise 2.2291 5 MVOOrientationNoise 2.0904 6 AccelerometerNoise 2.0903 6 GyroscopeNoise 2.0903 6 AccelerometerBiasNoise 2.0903 6 GyroscopeBiasNoise 2.0903 6 GPSPositionNoise 2.0903 6 GPSVelocityNoise 2.0141 6 MVOPositionNoise 1.9952 6 MVOOrientationNoise 1.8497 7 AccelerometerNoise 1.8497 7 GyroscopeNoise 1.8496 7 AccelerometerBiasNoise 1.8496 7 GyroscopeBiasNoise 1.8496 7 GPSPositionNoise 1.8398 7 GPSVelocityNoise 1.7528 7 MVOPositionNoise 1.7362 7 MVOOrientationNoise 1.5762 8 AccelerometerNoise 1.5762 8 GyroscopeNoise 1.5762 8 AccelerometerBiasNoise 1.5762 8 GyroscopeBiasNoise 1.5762 8 GPSPositionNoise 1.5762 8 GPSVelocityNoise 1.5107 8 MVOPositionNoise 1.4786 8 MVOOrientationNoise 1.3308 9 AccelerometerNoise 1.3308 9 GyroscopeNoise 1.3308 9 AccelerometerBiasNoise 1.3308 9 GyroscopeBiasNoise 1.3308 9 GPSPositionNoise 1.3308 9 GPSVelocityNoise 1.2934 9 MVOPositionNoise 1.2525 9 MVOOrientationNoise 1.1462 10 AccelerometerNoise 1.1462 10 GyroscopeNoise 1.1462 10 AccelerometerBiasNoise 1.1462 10 GyroscopeBiasNoise 1.1462 10 GPSPositionNoise 1.1443 10 GPSVelocityNoise 1.1332 10 MVOPositionNoise 1.0964 10 MVOOrientationNoise 1.0382 11 AccelerometerNoise 1.0382 11 GyroscopeNoise 1.0382 11 AccelerometerBiasNoise 1.0382 11 GyroscopeBiasNoise 1.0382 11 GPSPositionNoise 1.0348 11 GPSVelocityNoise 1.0348 11 MVOPositionNoise 1.0081 11 MVOOrientationNoise 0.9734 12 AccelerometerNoise 0.9734 12 GyroscopeNoise 0.9734 12 AccelerometerBiasNoise 0.9734 12 GyroscopeBiasNoise 0.9734 12 GPSPositionNoise 0.9693 12 GPSVelocityNoise 0.9682 12 MVOPositionNoise 0.9488 12 MVOOrientationNoise 0.9244 13 AccelerometerNoise 0.9244 13 GyroscopeNoise 0.9244 13 AccelerometerBiasNoise 0.9244 13 GyroscopeBiasNoise 0.9244 13 GPSPositionNoise 0.9203 13 GPSVelocityNoise 0.9199 13 MVOPositionNoise 0.9045 13 MVOOrientationNoise 0.8846 14 AccelerometerNoise 0.8846 14 GyroscopeNoise 0.8846 14 AccelerometerBiasNoise 0.8845 14 GyroscopeBiasNoise 0.8845 14 GPSPositionNoise 0.8807 14 GPSVelocityNoise 0.8807 14 MVOPositionNoise 0.8659 14 MVOOrientationNoise 0.8501 15 AccelerometerNoise 0.8501 15 GyroscopeNoise 0.8501 15 AccelerometerBiasNoise 0.8500 15 GyroscopeBiasNoise 0.8500 15 GPSPositionNoise 0.8457 15 GPSVelocityNoise 0.8453 15 MVOPositionNoise 0.8299 15 MVOOrientationNoise 0.8173 16 AccelerometerNoise 0.8173 16 GyroscopeNoise 0.8173 16 AccelerometerBiasNoise 0.8172 16 GyroscopeBiasNoise 0.8172 16 GPSPositionNoise 0.8122 16 GPSVelocityNoise 0.8116 16 MVOPositionNoise 0.7961 16 MVOOrientationNoise 0.7858 17 AccelerometerNoise 0.7858 17 GyroscopeNoise 0.7858 17 AccelerometerBiasNoise 0.7857 17 GyroscopeBiasNoise 0.7857 17 GPSPositionNoise 0.7807 17 GPSVelocityNoise 0.7800 17 MVOPositionNoise 0.7655 17 MVOOrientationNoise 0.7572 18 AccelerometerNoise 0.7572 18 GyroscopeNoise 0.7572 18 AccelerometerBiasNoise 0.7570 18 GyroscopeBiasNoise 0.7570 18 GPSPositionNoise 0.7525 18 GPSVelocityNoise 0.7520 18 MVOPositionNoise 0.7401 18 MVOOrientationNoise 0.7338 19 AccelerometerNoise 0.7337 19 GyroscopeNoise 0.7337 19 AccelerometerBiasNoise 0.7335 19 GyroscopeBiasNoise 0.7335 19 GPSPositionNoise 0.7293 19 GPSVelocityNoise 0.7290 19 MVOPositionNoise 0.7185 19 MVOOrientationNoise 0.7140 20 AccelerometerNoise 0.7138 20 GyroscopeNoise 0.7138 20 AccelerometerBiasNoise 0.7134 20 GyroscopeBiasNoise 0.7134 20 GPSPositionNoise 0.7086 20 GPSVelocityNoise 0.7068 20 MVOPositionNoise 0.6956 20 MVOOrientationNoise 0.6926 21 AccelerometerNoise 0.6922 21 GyroscopeNoise 0.6922 21 AccelerometerBiasNoise 0.6916 21 GyroscopeBiasNoise 0.6916 21 GPSPositionNoise 0.6862 21 GPSVelocityNoise 0.6822 21 MVOPositionNoise 0.6682 21 MVOOrientationNoise 0.6667 22 AccelerometerNoise 0.6660 22 GyroscopeNoise 0.6660 22 AccelerometerBiasNoise 0.6650 22 GyroscopeBiasNoise 0.6650 22 GPSPositionNoise 0.6605 22 GPSVelocityNoise 0.6541 22 MVOPositionNoise 0.6372 22 MVOOrientationNoise 0.6368 23 AccelerometerNoise 0.6356 23 GyroscopeNoise 0.6356 23 AccelerometerBiasNoise 0.6344 23 GyroscopeBiasNoise 0.6344 23 GPSPositionNoise 0.6324 23 GPSVelocityNoise 0.6252 23 MVOPositionNoise 0.6087 23 MVOOrientationNoise 0.6087 24 AccelerometerNoise 0.6075 24 GyroscopeNoise 0.6075 24 AccelerometerBiasNoise 0.6068 24 GyroscopeBiasNoise 0.6068 24 GPSPositionNoise 0.6061 24 GPSVelocityNoise 0.6032 24 MVOPositionNoise 0.6032 24 MVOOrientationNoise 0.6032 25 AccelerometerNoise 0.6017 25 GyroscopeNoise 0.6017 25 AccelerometerBiasNoise 0.6012 25 GyroscopeBiasNoise 0.6012 25 GPSPositionNoise 0.6010 25 GPSVelocityNoise 0.6005 25 MVOPositionNoise 0.6005 25 MVOOrientationNoise 0.6005 26 AccelerometerNoise 0.5992 26 GyroscopeNoise 0.5992 26 AccelerometerBiasNoise 0.5987 26 GyroscopeBiasNoise 0.5987 26 GPSPositionNoise 0.5983 26 GPSVelocityNoise 0.5983 26 MVOPositionNoise 0.5983 26 MVOOrientationNoise 0.5980 27 AccelerometerNoise 0.5973 27 GyroscopeNoise 0.5973 27 AccelerometerBiasNoise 0.5972 27 GyroscopeBiasNoise 0.5972 27 GPSPositionNoise 0.5970 27 GPSVelocityNoise 0.5970 27 MVOPositionNoise 0.5970 27 MVOOrientationNoise 0.5970 28 AccelerometerNoise 0.5970 28 GyroscopeNoise 0.5970 28 AccelerometerBiasNoise 0.5970 28 GyroscopeBiasNoise 0.5970 28 GPSPositionNoise 0.5970 28 GPSVelocityNoise 0.5970 28 MVOPositionNoise 0.5969 28 MVOOrientationNoise 0.5966 29 AccelerometerNoise 0.5966 29 GyroscopeNoise 0.5966 29 AccelerometerBiasNoise 0.5966 29 GyroscopeBiasNoise 0.5966 29 GPSPositionNoise 0.5966 29 GPSVelocityNoise 0.5966 29 MVOPositionNoise 0.5965 29 MVOOrientationNoise 0.5965 30 AccelerometerNoise 0.5965 30 GyroscopeNoise 0.5965 30 AccelerometerBiasNoise 0.5965 30 GyroscopeBiasNoise 0.5965 30 GPSPositionNoise 0.5964 30 GPSVelocityNoise 0.5964 30 MVOPositionNoise 0.5964 30 MVOOrientationNoise 0.5961 31 AccelerometerNoise 0.5961 31 GyroscopeNoise 0.5961 31 AccelerometerBiasNoise 0.5960 31 GyroscopeBiasNoise 0.5960 31 GPSPositionNoise 0.5960 31 GPSVelocityNoise 0.5960 31 MVOPositionNoise 0.5960 31 MVOOrientationNoise 0.5958 32 AccelerometerNoise 0.5956 32 GyroscopeNoise 0.5956 32 AccelerometerBiasNoise 0.5956 32 GyroscopeBiasNoise 0.5956 32 GPSPositionNoise 0.5955 32 GPSVelocityNoise 0.5955 32 MVOPositionNoise 0.5954 32 MVOOrientationNoise 0.5954 33 AccelerometerNoise 0.5954 33 GyroscopeNoise 0.5954 33 AccelerometerBiasNoise 0.5953 33 GyroscopeBiasNoise 0.5953 33 GPSPositionNoise 0.5953 33 GPSVelocityNoise 0.5953 33 MVOPositionNoise 0.5952 33 MVOOrientationNoise 0.5950 34 AccelerometerNoise 0.5949 34 GyroscopeNoise 0.5949 34 AccelerometerBiasNoise 0.5949 34 GyroscopeBiasNoise 0.5949 34 GPSPositionNoise 0.5948 34 GPSVelocityNoise 0.5947 34 MVOPositionNoise 0.5946 34 MVOOrientationNoise 0.5946 35 AccelerometerNoise 0.5946 35 GyroscopeNoise 0.5946 35 AccelerometerBiasNoise 0.5946 35 GyroscopeBiasNoise 0.5946 35 GPSPositionNoise 0.5946 35 GPSVelocityNoise 0.5945 35 MVOPositionNoise 0.5944 35 MVOOrientationNoise 0.5943 36 AccelerometerNoise 0.5942 36 GyroscopeNoise 0.5942 36 AccelerometerBiasNoise 0.5942 36 GyroscopeBiasNoise 0.5942 36 GPSPositionNoise 0.5941 36 GPSVelocityNoise 0.5941 36 MVOPositionNoise 0.5939 36 MVOOrientationNoise 0.5939 37 AccelerometerNoise 0.5939 37 GyroscopeNoise 0.5939 37 AccelerometerBiasNoise 0.5939 37 GyroscopeBiasNoise 0.5939 37 GPSPositionNoise 0.5939 37 GPSVelocityNoise 0.5939 37 MVOPositionNoise 0.5937 37 MVOOrientationNoise 0.5937 38 AccelerometerNoise 0.5937 38 GyroscopeNoise 0.5937 38 AccelerometerBiasNoise 0.5937 38 GyroscopeBiasNoise 0.5937 38 GPSPositionNoise 0.5936 38 GPSVelocityNoise 0.5935 38 MVOPositionNoise 0.5935 38 MVOOrientationNoise 0.5935 39 AccelerometerNoise 0.5935 39 GyroscopeNoise 0.5935 39 AccelerometerBiasNoise 0.5935 39 GyroscopeBiasNoise 0.5935 39 GPSPositionNoise 0.5934 39 GPSVelocityNoise 0.5934 39 MVOPositionNoise 0.5934 39 MVOOrientationNoise 0.5934 40 AccelerometerNoise 0.5934 40 GyroscopeNoise 0.5934 40 AccelerometerBiasNoise 0.5934 40 GyroscopeBiasNoise 0.5934 40 GPSPositionNoise 0.5933 40 GPSVelocityNoise 0.5933 40 MVOPositionNoise 0.5933 40 MVOOrientationNoise 0.5933
Предохраните данные датчика с помощью настроенного фильтра.
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,:), ... tunedmn.GPSPositionNoise,GPSVelocity(ii,:), ... tunedmn.GPSVelocityNoise); end if all(~isnan(MVOPosition(ii,1))) fusemvo(filter,MVOPosition(ii,:),tunedmn.MVOPositionNoise, ... MVOOrientation{ii},tunedmn.MVOOrientationNoise); end [posEstTuned(ii,:),qEstTuned(ii,:)] = pose(filter); end
Вычислите ошибки RMS.
orientationErrorTuned = rad2deg(dist(qEstTuned,Orientation)); rmsOrientationErrorTuned = sqrt(mean(orientationErrorTuned.^2))
rmsOrientationErrorTuned = 4.2335
positionErrorTuned = sqrt(sum((posEstTuned - Position).^2,2)); rmsPositionErrorTuned = sqrt(mean( positionErrorTuned.^2))
rmsPositionErrorTuned = 0.1024
Визуализация результатов.
figure; t = (0:N-1)./filter.IMUSampleRate; subplot(2,1,1) plot(t, positionErrorTuned,'b'); title("Tuned insfilterErrorState" + 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
- Объект фильтраinsfilterErrorState
объектОбъект фильтра, заданный как insfilterErrorState
объект.
measureNoise
- Измерительный шумИзмерительный шум, заданный как структура. Функция использует вход шума измерения в качестве начального предположения для настройки шума измерения. Структура должна содержать следующие поля:
Имя поля | Описание |
---|---|
MVOOrientationNoise | Ковариация измерения ориентации монокулярной визуальной одометрии, заданная в виде скаляра, 3-элементного вектора или матрицы 3 на 3 в рад2 |
MVOPositionNoise | Ковариация измерения положения MVO, заданная в виде скаляра, 3-элементного вектора или матрицы 3 на 3 в m2 |
GPSPositionNoise | Отклонение шума положения GPS, заданная как скаляр в m2 |
GPSVelocityNoise | Отклонение шума скорости GPS, заданная как скаляр в (м/с)2 |
sensorData
- Данные о датчикеtable
Данные о датчике, заданные как таблица. В каждой строке данные о датчике задаются как:
Accelerometer
- Данные акселерометра, заданные как вектор скаляров 1 на 3 в m2/ с .
Gyroscope
- Данные гироскопа, заданные как вектор 1 на 3 скаляров в рад/с.
MVOOrienation
- Ориентация камеры относительно локальной навигационной системы координат, заданная как скалярный кватернион или матрица вращения 3 на 3. Кватернион или матрица поворота является поворотом кадра от локального навигационного кадра к текущей системе координат камеры.
MVOPosition
- Положение камеры в локальной навигационной системе координат, заданное как действительный 3-элементный вектор-строка в метрах.
GPSPosition
- данные положения GPS, заданные как вектор 1 на 3 скаляров в метрах.
GPSVelocity
- данные скорости GPS, заданные как вектор 1 на 3 скаляров в м/с.
Если GPS-датчик не производит полных измерений, задайте соответствующую запись для GPSPosition
и/или GPSVelocity
как NaN
. Если вы задаете Cost
свойство входа строения тюнера, config
, в Custom
, тогда можно использовать другие типы данных для sensorData
вход, основанный на вашем выборе.
groundTruth
- Достоверные данныеtable
Достоверные данные, заданные как table
. В каждой строке таблица может опционально содержать любую из следующих переменных:
Orientation
- Ориентация от навигационной системы координат к каркасу кузова, заданная как quaternion
или матрица вращения 3 на 3.
Position
- Положение в навигационной системе координат, заданное как вектор 1 на 3 скаляров в метрах.
Velocity
- Скорость в навигационной системе координат, заданная как вектор 1 на 3 скаляров в м/с.
AccelerometerBias
- Смещение угла дельты акселерометра в каркасе кузова, заданное как вектор скаляров 1 на 3 в m2/ с .
VisualOdometryScale
- Масштабный коэффициент визуальной одометрии, заданный как скаляр.
Функция обрабатывает каждую строку sensorData
и groundTruth
таблицы для вычисления оценки состояния и ошибки RMS из основной истины. Переменные состояния, отсутствующие в groundTruth
входы для сравнения игнорируются. The sensorData
и groundTruth
таблицы должны иметь одинаковое число строк.
Если вы задаете Cost
свойство входа строения тюнера, config
, в Custom
, тогда можно использовать другие типы данных для groundTruth
вход, основанный на вашем выборе.
config
- Настройка тюнераtunerconfig
объектСтроение тюнера, заданный как tunerconfig
объект.
tunedMeasureNoise
- Настроенный шум измеренияНастроенный шум измерения, возвращенный как структура. Структура содержит эти поля.
Имя поля | Описание |
---|---|
MVOOrientationNoise | Ковариация измерения ориентации монокулярной визуальной одометрии, заданная в виде скаляра, 3-элементного вектора или матрицы 3 на 3 в рад2 |
MVOPositionNoise | Ковариация измерения положения MVO, заданная в виде скаляра, 3-элементного вектора или матрицы 3 на 3 в m2 |
GPSPositionNoise | Отклонение шума положения GPS, заданная как скаляр в m2 |
GPSVelocityNoise | Отклонение шума скорости GPS, заданная как скаляр в (м/с)2 |
[1] Abbeel, P., Coates, A., Montemerlo, M., Ng, A.Y. and Thrun, S. Discriminative Training of Фильтры Калмана. In Robotics: Science and systems, Vol. 2, pp. 1, 2005.
У вас есть измененная версия этого примера. Вы хотите открыть этот пример с вашими правками?
1. Если смысл перевода понятен, то лучше оставьте как есть и не придирайтесь к словам, синонимам и тому подобному. О вкусах не спорим.
2. Не дополняйте перевод комментариями “от себя”. В исправлении не должно появляться дополнительных смыслов и комментариев, отсутствующих в оригинале. Такие правки не получится интегрировать в алгоритме автоматического перевода.
3. Сохраняйте структуру оригинального текста - например, не разбивайте одно предложение на два.
4. Не имеет смысла однотипное исправление перевода какого-то термина во всех предложениях. Исправляйте только в одном месте. Когда Вашу правку одобрят, это исправление будет алгоритмически распространено и на другие части документации.
5. По иным вопросам, например если надо исправить заблокированное для перевода слово, обратитесь к редакторам через форму технической поддержки.