How to interpolate GPS data for use in Kalman Filter?

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.

Answer 1

[JangHo Cho]

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.

Answer 2

[Ryan Salvo]

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.