How to interpolate GPS data for use in Kalman Filter?

15 次查看（过去 30 天）
Marin Aldimirov
Marin Aldimirov 2016-5-13
回答： Ryan Salvo 2022-5-27
I have a Kalman Filter for inertial navigation, that takes in 6 axis INS and 3 axis GPS data and returns the XYZ position best estimate. The INS data is sampled with Fs=500Hz => dT=2ms and the GPS data is sampled at 1Hz => 1s. The device I use maps the GPS samples within 2-4ms accuracy to the corresponding INS sample.
Is there a way to interpolate the GPS data so that I can have a correct 2Hz or 4Hz GPS signal or even at 500Hz?
By correct I mean that the GPS data points would be equally distant in time, but not in space, according to the current acceleration/speed. So I was thinking of running the Filter once to get an estimate of the speed and then use it to interpolate the GPS data. Am I thinking it right and how can the acceleration/speed based interpolation be done in Matlab code?
EDIT: The INS and GPS sensors are not simulated. I am using real data from the device sitting in my car. So far I thought of this algorithm:
1. Run the data trough the Kalman Filter
2. Calculate the total distance traveled, by summing each sample to sample distance
3. Use each sample to sample distance to calculate the query points for the interpolated GPS data
% xyz - the KF calculated positions in reference frame
% gps - the GPS coordinates transformed in XYZ reference frame with lla2flat
function gpsI = interpolateGps(xyz, gps)
gpsCount = length(gps);
totalSeconds = gpsCount - 1;
t = 0 : totalSeconds;
xyzCount = length(xyz);
xyzLens = zeros(xyzCount,1);
for i = 2 : xyzCount
xyzLens(i) = norm(xyz(i,:)-xyz(i-1,:));
end
totalLength = sum(xyzLens);
tq = zeros(xyzCount,1);
for i = 2 : xyzCount
tq(i) = sum(xyzLens(1:i)) / totalLength;
end
tq = tq * totalSeconds;
gpsI = zeros(xyzCount,3);
gpsI(:,1) = interp1(t,gps(:,1),tq,'spline');
gpsI(:,2) = interp1(t,gps(:,2),tq,'spline');
gpsI(:,3) = interp1(t,gps(:,3),tq,'spline');
end
This code works as it returns a 3D line that overlaps pretty good the xyz 3D line, but I still don't get the sample points from xyz and gpsI to be at the same places.

请先登录，再回答此问题。

回答（2 个）

JangHo Cho
JangHo Cho 2016-5-16
Can you show the model of INS sensor(used in Kalman Filter?) You don't need to show all the MATLAB code, if you use Simulink and you modeled it in Simulink, I might not help you.
Ryan Salvo
Ryan Salvo 2022-5-27
You can combine the IMU and GPS data with the insfilterAsync object. This will allow you to combine the two sensors at the original sampling rates of 500 Hz and 1 Hz.

类别

在 Help Center 和 File Exchange 中查找有关 Automated Driving Toolbox 的更多信息

标签

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by