tune
Ajuste los parámetros insfilterErrorState
para reducir el error de estimación
Desde R2021a
Sintaxis
Descripción
ajusta las propiedades del objeto de filtro tunedMeasureNoise
= tune(filter
,measureNoise
,sensorData
,groundTruth
)insfilterErrorState
, filter
y los ruidos de medición para reducir la raíz cuadrática media (RMS) error de estimación de estado entre los datos del sensor fusionado y ground-truth. La función también devuelve el ruido de medición ajustado, tunedMeasureNoise
. La función utiliza los valores de propiedad en el filtro y el ruido de medición proporcionado en la estructura measureNoise
como estimación inicial para el algoritmo de optimización.
especifica la configuración de ajuste basada en un objeto tunedMeasureNoise
= tune(___,config
)tunerconfig
, config
.
Ejemplos
Ajuste insfilterErrorState
para optimizar la estimación de pose
Cargue los datos registrados del sensor y los datos ground-truth.
load('insfilterErrorStateTuneData.mat');
Cree tablas para los datos del sensor y los datos de verdad.
sensorData = table(Accelerometer,Gyroscope, ... GPSPosition,GPSVelocity,MVOOrientation, ... MVOPosition); groundTruth = table(Orientation,Position);
Cree un objeto de filtro insfilterErrorState
.
filter = insfilterErrorState('State',initialState, ... 'StateCovariance',initialStateCovariance);
Cree un objeto de configuración de ajustador para el filtro. Utilice la función de ruido del ajustador para obtener un conjunto de ruidos iniciales del sensor utilizados en el filtro.
cfg = tunerconfig('insfilterErrorState','MaxIterations',40); measNoise = tunernoise('insfilterErrorState')
measNoise = struct with fields:
MVOOrientationNoise: 1
MVOPositionNoise: 1
GPSPositionNoise: 1
GPSVelocityNoise: 1
Ajusteel filtro y obtenga los parámetros ajustados.
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.5983 27 AccelerometerNoise 0.5975 27 GyroscopeNoise 0.5975 27 AccelerometerBiasNoise 0.5974 27 GyroscopeBiasNoise 0.5974 27 GPSPositionNoise 0.5973 27 GPSVelocityNoise 0.5972 27 MVOPositionNoise 0.5971 27 MVOOrientationNoise 0.5971 28 AccelerometerNoise 0.5971 28 GyroscopeNoise 0.5971 28 AccelerometerBiasNoise 0.5970 28 GyroscopeBiasNoise 0.5970 28 GPSPositionNoise 0.5970 28 GPSVelocityNoise 0.5970 28 MVOPositionNoise 0.5970 28 MVOOrientationNoise 0.5970 29 AccelerometerNoise 0.5970 29 GyroscopeNoise 0.5970 29 AccelerometerBiasNoise 0.5970 29 GyroscopeBiasNoise 0.5970 29 GPSPositionNoise 0.5970 29 GPSVelocityNoise 0.5970 29 MVOPositionNoise 0.5970 29 MVOOrientationNoise 0.5970 30 AccelerometerNoise 0.5969 30 GyroscopeNoise 0.5969 30 AccelerometerBiasNoise 0.5969 30 GyroscopeBiasNoise 0.5969 30 GPSPositionNoise 0.5969 30 GPSVelocityNoise 0.5969 30 MVOPositionNoise 0.5968 30 MVOOrientationNoise 0.5968 31 AccelerometerNoise 0.5968 31 GyroscopeNoise 0.5968 31 AccelerometerBiasNoise 0.5968 31 GyroscopeBiasNoise 0.5968 31 GPSPositionNoise 0.5968 31 GPSVelocityNoise 0.5968 31 MVOPositionNoise 0.5967 31 MVOOrientationNoise 0.5967 32 AccelerometerNoise 0.5967 32 GyroscopeNoise 0.5967 32 AccelerometerBiasNoise 0.5967 32 GyroscopeBiasNoise 0.5967 32 GPSPositionNoise 0.5967 32 GPSVelocityNoise 0.5967 32 MVOPositionNoise 0.5966 32 MVOOrientationNoise 0.5966 33 AccelerometerNoise 0.5966 33 GyroscopeNoise 0.5966 33 AccelerometerBiasNoise 0.5966 33 GyroscopeBiasNoise 0.5966 33 GPSPositionNoise 0.5966 33 GPSVelocityNoise 0.5966 33 MVOPositionNoise 0.5965 33 MVOOrientationNoise 0.5965 34 AccelerometerNoise 0.5965 34 GyroscopeNoise 0.5965 34 AccelerometerBiasNoise 0.5965 34 GyroscopeBiasNoise 0.5965 34 GPSPositionNoise 0.5965 34 GPSVelocityNoise 0.5964 34 MVOPositionNoise 0.5964 34 MVOOrientationNoise 0.5964 35 AccelerometerNoise 0.5964 35 GyroscopeNoise 0.5964 35 AccelerometerBiasNoise 0.5963 35 GyroscopeBiasNoise 0.5963 35 GPSPositionNoise 0.5963 35 GPSVelocityNoise 0.5963 35 MVOPositionNoise 0.5963 35 MVOOrientationNoise 0.5963 36 AccelerometerNoise 0.5963 36 GyroscopeNoise 0.5963 36 AccelerometerBiasNoise 0.5963 36 GyroscopeBiasNoise 0.5963 36 GPSPositionNoise 0.5963 36 GPSVelocityNoise 0.5963 36 MVOPositionNoise 0.5963 36 MVOOrientationNoise 0.5963 37 AccelerometerNoise 0.5963 37 GyroscopeNoise 0.5963 37 AccelerometerBiasNoise 0.5963 37 GyroscopeBiasNoise 0.5963 37 GPSPositionNoise 0.5962 37 GPSVelocityNoise 0.5962 37 MVOPositionNoise 0.5962 37 MVOOrientationNoise 0.5962 38 AccelerometerNoise 0.5962 38 GyroscopeNoise 0.5962 38 AccelerometerBiasNoise 0.5962 38 GyroscopeBiasNoise 0.5962 38 GPSPositionNoise 0.5962 38 GPSVelocityNoise 0.5961 38 MVOPositionNoise 0.5961 38 MVOOrientationNoise 0.5961 39 AccelerometerNoise 0.5961 39 GyroscopeNoise 0.5961 39 AccelerometerBiasNoise 0.5961 39 GyroscopeBiasNoise 0.5961 39 GPSPositionNoise 0.5961 39 GPSVelocityNoise 0.5960 39 MVOPositionNoise 0.5960 39 MVOOrientationNoise 0.5960 40 AccelerometerNoise 0.5960 40 GyroscopeNoise 0.5960 40 AccelerometerBiasNoise 0.5960 40 GyroscopeBiasNoise 0.5960 40 GPSPositionNoise 0.5960 40 GPSVelocityNoise 0.5959 40 MVOPositionNoise 0.5959 40 MVOOrientationNoise 0.5959
Fusione los datos del sensor utilizando el filtro ajustado.
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
Calcule los errores RMS.
orientationErrorTuned = rad2deg(dist(qEstTuned,Orientation)); rmsOrientationErrorTuned = sqrt(mean(orientationErrorTuned.^2))
rmsOrientationErrorTuned = 4.4999
positionErrorTuned = sqrt(sum((posEstTuned - Position).^2,2)); rmsPositionErrorTuned = sqrt(mean( positionErrorTuned.^2))
rmsPositionErrorTuned = 0.1172
Visualiza los resultados.
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)');
Argumentos de entrada
filter
— Objeto de filtro
insfilterErrorState
objeto
Objeto de filtro, especificado como objeto insfilterErrorState
.
measureNoise
— Ruido de medición
estructura
Ruido de medición, especificado como estructura. La función utiliza la entrada de ruido de medición como estimación inicial para ajustar el ruido de medición. La estructura debe contener estos campos:
Nombre del campo | Descripción |
---|---|
MVOOrientationNoise | Covarianza de medición de orientación de odometría visual monocular, especificada como escalar, vector de 3 elementos o matriz de 3 por 3 en rad2 |
MVOPositionNoise | Covarianza de medición de posición de MVO, especificada como escalar, vector de 3 elementos o matriz de 3 por 3 en m2 |
GPSPositionNoise | Variación del ruido de la posición GPS, especificada como un escalar en m2 |
GPSVelocityNoise | Variación del ruido de la velocidad del GPS, especificada como un escalar en (m/s) 2 |
sensorData
— Datos del sensor
table
Datos del sensor, especificados como una tabla. En cada fila, los datos del sensor se especifican como:
Accelerometer
— Datos del acelerómetro, especificados como un vector de escalares de 1 por 3 en m2/s.Gyroscope
— Datos del giroscopio, especificados como un vector de escalares de 1 por 3 en rad/s.MVOOrienation
— Orientación de la cámara con respecto al marco de navegación local, especificada como un cuaternión escalar o una matriz de rotación de 3 por 3. El cuaternión o matriz de rotación es una rotación de cuadro desde el cuadro de navegación local hasta el sistema de coordenadas de la cámara actual.MVOPosition
— Posición de la cámara en el marco de navegación local, especificada como un vector fila real de 3 elementos en metros.GPSPosition
: datos de posición GPS, especificados como un vector de 1 por 3 de latitud en grados, longitud en grados y altitud en metros.GPSVelocity
— Datos de velocidad GPS, especificados como un vector de escalares de 1 por 3 en m/s.
Si el sensor GPS no produce mediciones completas, especifique la entrada correspondiente para GPSPosition
y/o GPSVelocity
como NaN
. Si establece la propiedad Cost
de la entrada de configuración del ajustador, config
, en Custom
, entonces puede usar otros tipos de datos para sensorData
entrada según su elección.
groundTruth
— Datos ground-truth
table
Datos ground-truth, especificados como table
. En cada fila, la tabla puede contener opcionalmente cualquiera de estas variables:
Orientation
— Orientación desde el marco de navegación hasta el marco del cuerpo, especificada comoquaternion
o una matriz de rotación de 3 por 3.Position
: posición en el marco de navegación, especificada como un vector de escalares de 1 por 3 en metros.Velocity
— Velocidad en el marco de navegación, especificada como un vector de escalares de 1 por 3 en m/s.AccelerometerBias
— Sesgo del ángulo delta del acelerómetro en la estructura del cuerpo, especificado como un vector de escalares de 1 por 3 en m2/s.VisualOdometryScale
— Factor de escala de odometría visual, especificado como escalar.
La función procesa cada fila de las tablas sensorData
y groundTruth
secuencialmente para calcular la estimación del estado y el error RMS a partir de ground-truth. Las variables de estado que no están presentes en la entrada groundTruth
se ignoran para la comparación. Las tablas sensorData
y groundTruth
deben tener el mismo número de filas.
Si establece la propiedad Cost
de la entrada de configuración del ajustador, config
, en Custom
, entonces puede usar otros tipos de datos para la groundTruth
entrada según su elección.
config
— Configuración del ajustador
tunerconfig
objeto
Configuración del ajustador, especificada como un objeto tunerconfig
.
Argumentos de salida
tunedMeasureNoise
— Ruido de medición ajustado
estructura
Ruido de medición ajustado, devuelto como una estructura. La estructura contiene estos campos.
Nombre del campo | Descripción |
---|---|
MVOOrientationNoise | Covarianza de medición de orientación de odometría visual monocular, especificada como escalar, vector de 3 elementos o matriz de 3 por 3 en rad2 |
MVOPositionNoise | Covarianza de medición de posición de MVO, especificada como escalar, vector de 3 elementos o matriz de 3 por 3 en m2 |
GPSPositionNoise | Variación del ruido de la posición GPS, especificada como un escalar en m2 |
GPSVelocityNoise | Variación del ruido de la velocidad del GPS, especificada como un escalar en (m/s) 2 |
Referencias
[1] Abbeel, P., Coates, A., Montemerlo, M., Ng, A.Y. and Thrun, S. Discriminative Training of Kalman Filters. In Robotics: Science and systems, Vol. 2, pp. 1, 2005.
Historial de versiones
Introducido en R2021a
Consulte también
Comando de MATLAB
Ha hecho clic en un enlace que corresponde a este comando de MATLAB:
Ejecute el comando introduciéndolo en la ventana de comandos de MATLAB. Los navegadores web no admiten comandos de MATLAB.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)