El propósito del artículo no es explicar los principios del filtro de Kalman, sino demostrarlo utilizando datos reales (en bruto) como ejemplo. Aquellos que lo deseen pueden modificar el código fuente y experimentar con el algoritmo, espero que mi trabajo ayude a aquellos que se enfrentan a una tarea similar.
Los datos utilizados provienen de un receptor GPS en el formato NMEA-0183, en particular los mensajes GGA y VTG.
El filtrado es necesario debido al ruido del GPS. Las causas de la interferencia en los datos del GPS son diferentes. Los principales:
- interferencias.
- Obstáculos a la señal.
- Posición de órbita GPS. Por ejemplo, la baja inclinación de las órbitas GPS (aproximadamente 55 °) perjudica gravemente la precisión en las regiones circumpolares de la Tierra.
Todo esto en total conduce a saltos en la posición, desplazamientos del curso y otros problemas. Y en el trabajo, antes que nada, necesitaba obtener exactamente la velocidad filtrada.
El hecho es que la velocidad medida por el equipo y transmitida en el mensaje VTG dio lecturas inverosímiles (saltos, etc.), lo que complicó extremadamente las tareas de control.
Por lo tanto, se decidió construir un modelo de filtro en Octave, y habiendo recibido la velocidad como derivada de los datos GPS representados por el mensaje GGA, compárelos con los datos de velocidad originales del mensaje VTG.
Para facilitar la comparación, los datos deben mostrarse en un gráfico.
El filtro Kalman hace un gran trabajo al filtrar datos y sus derivados.
«».
— . ( ) : - - . , Predict, Update, Invariant :
https://habr.com/ru/post/140274/
https://habr.com/ru/post/166693/
https://ru.wikipedia.org/wiki/%D0%A4%D0%B8%D0%BB%D1%8C%D1%82%D1%80_%D0%9A%D0%B0%D0%BB%D0%BC%D0%B0%D0%BD%D0%B0
Octave (open-source Matlab Linux). F . ( ):
, .
— (PAC ) C++.
— Octave .
++ , , Octave — .
GGA :
VTG :
«--» — , . Latitude Longitude.
.
, .:
Fk — //, state-transition model.
Hk — /, observation model.
Qk — , covariance of the process noise;.
Rk — , covariance of the observation noise.
K — .
Xp — () .
Xk — .
X(k-1) — .
Pp — , .
P — .
P(k-1) — () .
z- () .
:
Xp = Fk X(k-1); .
Pp = Fk P(k-1) * F'k + Q; .
:
K = Pp H' inv(H Pp H' + R); .
Xk = Xp + K(z — HXp); z.
P = Pp — KHPp; .
.
KalmanX KalmanY .
X Y , , Q, R, F reset ( Q R, ).
(habrGGA.m) , , .
Q, R . ( Q, R). , .
, :
( ) , Q. R = 0,2.
, :
— X Y (Latitude Longitude) , :
:
( VTG) , .
Q R, (habrGGA.m):
if j==1 Q = [ 0.01 0;
0 0.01];
Q1=Q;
R=0.2; R1=R;
elseif j==2 Q = [ 0.05 0;
0 0.05];
Q2=Q;
R=0.2; R1=R;
elseif j==3 Q = [1 0;
0 1];
Q3=Q;
R=0.2; R1=R;
end
Para mis propósitos, el valor promedio de Q (0.05) ahora parece ser el más preferible en este kit NMEA. Se implementará una selección de coeficientes más precisa y posiblemente dinámicamente variable durante la depuración en los ensayos de amarre.
Archivos adjuntos.
habrGGA.m: el script principal (lectura de datos, invocación de la función de filtro, trazado de gráficos)
KalmanX.m: filtro de Kalman (latitud)
KalmanY.m: filtro de Kalman (longitud)