KITTI IMU data gaps interpolation

Question

I am using some IMU data from KITTI raw dataset. There are gaps in OxTs IMU data. I want to interpolate the gaps. The data looks periodically. Is there any way to interpolate them realistically? Below is the visualization of accelerations at XYZ axis.

I tried to fit the data with a trigonometric function, however it was not good enough.

def trigonometric_model2(t, a1, b1, a2, b2, a3, b3, a4, b4, a5, b5, a6, b6, a7, b7, a8, b8, c):
    return (a1 * np.sin(b1 * t) + a2 * np.cos(b2 * t) + 
            a3 * np.sin(b3 * t) + a4 * np.cos(b4 * t) + 
            a5 * np.sin(b5 * t) + a6 * np.cos(b6 * t) + 
            a7 * np.sin(b7 * t) + a8 * np.cos(b8 * t) + 
            c)
# data is a n * 4 array. 
# The first col is the timestamps,
# The second col is the acceleration at x direction
# The third col is the accelration at the y direction
# The last col is the acceleration at the z direction 
ts = data[:, 0] # there is an ~2 second gap as shown above 
axs = data[:, 1]

# try to fit the model on the data
params, _ = curve_fit(trigonometric_model2, ts, axs, maxfev=60000)

# predict the data for the gap
xs = np.linspace(ts[0], ts[-1], 500)
ys = trigonometric_model2(xs, *params)

Here is the sample data https://docs.google.com/spreadsheets/d/1NSjnnPQR-p2iB-EYGVJw6XLkzfMQ5XNz6dmj8WaJElM/edit?usp=sharing

Answer 1

Several things to improve here:

1. be sure to scale your data before passing into the model. For example divide your time axis by mean value xs / np.max(xs)
2. Increase number of sin/cos parameters. Ideally you can tune parameter N - which influence number of sin/cos that you will use.
3. convergence can be improved if you'll provide good initiall guess for the parameters

import numpy as np
from scipy.optimize import curve_fit


def trig_model(t, *args):
    """
        args have 2*N + 1 coefficients:
        - constant
        - 2*N coefficients for sin/cos
    """
    c, args = args[0], args[1:]
    n = len(args) // 2 

    res = c
    for i in range(n):
        # smallest period is when i=0 and is equal to T = 2 
        # so the data `t` should either be inside interval [0,2]
        # or have period less less or equal 2.

        res += args[2*i] * np.sin(np.pi * t * (1+i)) + \
               args[2*i+1] * np.cos(np.pi * t *(1+i))

    
    return res


xs = df['t']
ys = df['ax']

# intial guess of the parameters
n = 8
params0 = np.random.random(2*n+1) 
params0[0] = np.mean(ys) # constant

# scalling the data
xs = xs / np.max(xs)

params, _ = curve_fit(trig_model, xs,ys, p0 = params0)
y_pred = trig_model(xs, *params)

params

Result with n=8

Result with n=20