The purpose of the article is not to explain the principles of the Kalman filter, but to demonstrate it using real (raw) data as an example. Those who wish can modify the source code and experiment with the algorithm, I hope that my work will help those who are faced with a similar task.
The data used is from a GPS receiver in the NMEA-0183 format, in particular the GGA and VTG messages.
Filtering is necessary due to the noise of the GPS. The causes of interference in GPS data are different. The main ones:
- atmospherics.
- obstacles to the signal.
- GPS orbit position. For example, the low inclination of GPS orbits (approximately 55 °) seriously impairs accuracy in the circumpolar regions of the Earth.
All this in total leads to jumps in position, course offsets, and other troubles. And in the work, first of all, I needed to get exactly the filtered speed.
The fact is that the speed measured by the equipment and transmitted in the VTG message gave implausible readings (jumps, etc.), which extremely complicated the control tasks.
Therefore, it was decided to build a filter model in Octave, and having received the speed as a derivative of the GPS data represented by the GGA message, compare it with the original speed data from the VTG message.
For ease of comparison, the data must be displayed on one graph.
The Kalman filter does a great job of filtering data and its derivatives.
«».
— . ( ) : - - . , 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
For my purposes, the average Q value (0.05) now seems to be the most preferable on this NMEA kit. A more accurate, and possibly dynamically variable selection of coefficients will be implemented during debugging at mooring tests.
Attached files.
habrGGA.m - the main script (reading data, calling the filter function, plotting graphs)
KalmanX.m - Kalman filter (Latitude)
KalmanY.m - Kalman filter (Longitude)