Stuck at upsampling trajectory data to higher sampling frequency - shift

Hi all,

I am trying to upsample some trajectory data from my motion capture dataset (500Hz) to match my sensor dataset (5000Hz). To obtain the sensordata frame that would correspond to a specific motion capture frame, let's say, 31, I am doing 31 * (5000/500) - 9 (or (31-1)*10 + 1 would work, too). I obtained the following code from a supervisor(who is not available the coming weeks) and I tweaked it a bit because I found some errors. But recently I found that the trajectory data shifts negatively, so for marker Fc (x position), the original position at frame 1 is 0 (in the motion capture dataset), but in the interpolated data at frame 1 it is minus something. And this is also the case for the last frame.

This is my complete code, but I am having trouble with the interpolate_moca() function. The plots below illustrate what I mean. Thanks in advance for taking a look at it with me!

l = load("C:\Users\puckb\Documents\Study\2024-2025\Graduation\Graduation\4Results\Code\Fietskar-dataprocessing-20250618\Fietskar-dataprocessing-20250618\01-matfiles\anal_general_20250729120951.mat");
file_anal = l.file_anal;
S = l.S;
M = l.M;
t = 47; % Trial number
i_mstart_t = file_anal.start_index_x(t); 
i_mend_t   = file_anal.stop_index_x(t);
i_sstart_t = (i_mstart_t*10) - 9; % convert start index to sensor frame
i_send_t = (i_mend_t*10) - 9; % convert stop index to sensor frame
i_Iend_t = i_send_t - i_sstart_t + 1;
time = dm.time(i_mstart_t:i_mend_t);
time_s = ds.Time(i_sstart_t:i_send_t);
fco_moca_forICOR = 30; % Filter cut-off frequency (Hz) used to refilter the fitted spline used to interpolate the moca data.

% Trial specifics
trial_name = file_anal.filename_moca{t}; trial_label = trial_name(1:end-4);
dm = M.(trial_label);
ds = S.(trial_label);
labels = dm.labels;
Fc_nam = 'Fc';
Fc_id = find(strcmp(labels, Fc_nam));
disp(Fc_id);
fs_sens = 5000;
fs_moca = 500;

% Interpolating trajectory
traj_all = dm.trajectories;
traj = traj_all(:,:,i_mstart_t:i_mend_t);
traj_intp = interpolate_moca(traj,time,time_s,fs_sens,fs_moca,fco_moca_forICOR);

% Trajectory original Fc x en z
%not interpolated
F_glob_x = squeeze(dm.trajectories(Fc_id, 1, i_mstart_t:i_mend_t));
F_glob_y = squeeze(dm.trajectories(Fc_id, 2, i_mstart_t:i_mend_t)); 
F_glob_z = squeeze(dm.trajectories(Fc_id, 3, i_mstart_t:i_mend_t));

% Interpolated
Fc_x_int = squeeze(traj_intp(Fc_id, 1,:)); % Get arrays for x, y, z trajectories of desired markers, 
Fc_y_int = squeeze(traj_intp(Fc_id, 2,:)); % between start and stop frames for sensor (calculated 
Fc_z_int = squeeze(traj_intp(Fc_id, 3,:)); % based on moca startstop indices

% Plotting
f = figure;
subplot(2,1,1);
plot(F_glob_x,F_glob_z);

subplot(2,1,2);
plot(Fc_x_int,Fc_z_int);

function traj_intp = interpolate_moca(traj,time,time_s,fs_sens,fs_moca,fco_moca_forICOR)
% Interpolate data
    r = fs_sens/fs_moca; % Sample rate increase factor, must be multiplied with moca frames, and then -9
    traj_intp = nan(size(traj,1),size(traj,2),((r*size(traj,3))-9)); % PUCK EDIT changed r*size(traj,3) into ((r*size(traj,3))-9) 
    traj_og = traj;
    Fc_x_int = squeeze(traj(Fc_id, 1,:)); % Get arrays for x, y, z trajectories of desired markers, 
    disp([time(1), time(end)]);
    disp([time_s(1), time_s(end)]);
    for mar = 1:size(traj,1)
        % Get trajectory data
        xd_og = squeeze(traj(mar,1,:));
        yd_og = squeeze(traj(mar,2,:));
        zd_og = squeeze(traj(mar,3,:));
        ns = size(xd_og,1); %number of samples
        ins = (r*ns)-9; % interpolated number of samples
        % Corresponding timevector
        t_og = time;
        %t_in = interp(t_og,r);
        t_in = time_s; % PUCK EDIT added actual timevector of sensordata, of course trimmed between startstop
        % Find nans
        xxnan = double(isnan(xd_og));
        yynan = double(isnan(yd_og));
        zznan = double(isnan(zd_og));
        % xyznan = [xxnan,yynan,zznan];
        xxnan_in = interp1(t_og, xxnan,t_in,'linear');
        yynan_in = interp1(t_og, yynan,t_in,'linear');
        zznan_in = interp1(t_og, zznan,t_in,'linear');
        xyznan_in = [xxnan_in,yynan_in,zznan_in];
        xyznan_in(xyznan_in<=0.4) = 0;
        xyznan_in(xyznan_in>0.4) = 1;

        % For these, linearly interpolate where the nans were. We will be replacing these by nans again later,
        % but we cannot interpolate with duplicates in the query vector
        xd = fillmissing(xd_og,"pchip");
        yd = fillmissing(yd_og,"pchip");
        zd = fillmissing(zd_og,"pchip");
        % Interpolate
        xx = interpft(xd,ins); % PUCK EDIT replaced r*ns by ins
        yy = interpft(yd,ins); % PUCK EDIT replaced r*ns by ins
        zz = interpft(zd,ins); % PUCK EDIT replaced r*ns by ins
        % Add NaNs back in
        xx((xyznan_in(:,1))==1) = NaN;
        yy((xyznan_in(:,2))==1) = NaN;
        zz((xyznan_in(:,3))==1) = NaN;
        % Refilter: These will be used to determine the ICOR, so we filter them with a low threshold, otherwise we will get all-over-the-place estimates for the ICOR
        xx = filt_moca(xx, fco_moca_forICOR, fs_sens);
        yy = filt_moca(yy, fco_moca_forICOR, fs_sens);
        zz = filt_moca(zz, fco_moca_forICOR, fs_sens);
        % Add into complete trajectory (stored variable)
        traj_intp(mar,1:3,:) = [xx;yy;zz];
        
    end
end

function filtered_traj = filt_moca(traj, fco_moca, fs_moca)
% Filters the motion capture data using cutoff frequency
    % Filter inputs: 6th order butterworth filter results in 12th order filter
    [B,A] = butter(6, (fco_moca/(fs_moca/2)), 'low');
    nan_is = isnan(traj);
    not_nan_is = ~isnan(traj);
    x_og_nonan = traj(not_nan_is);
    x_nonan = filtfilt(B,A, x_og_nonan);
    x = traj; x(not_nan_is) = x_nonan; 
    filtered_traj = x';
end

[dpb]

Why not just call resample?

But, you forgot to attach a dataset for anybody to poke at...

[Star Strider]

I have no idea what 'file_anal' is.

If it is a table and has associated times, one option woould be to convert it to a timetable and then use the retime function.

[Puck]

Hey, thanks for the replies, I really appreciate it. I would rather not share the dataset because it is confidential, but I understand that this of course makes it a bit difficult to help, sorry for the inconvenience.

file_anal is a table containing file info and analysis info.

About the dataset: 'traj' is a 3D array that contains trajectories for each marker that I have: (# markers)x(4)x(number of frames). The second dimension consists of X, Y, Z, and the 4th position is empty in this case.

I realised that there is not a shift of the data, but rather that the beginning and the end of the interpolated dataset are not at the 'positions' they should be. I already changed interpft into interp1, and this helped a lot. But right now, the problem seems to lie in my refilter step (so after interpolating)

Some info about the inputs and outputs of interpolate_moca function:

before interpolating: xd(1) = 4.465627, xd_og(end) = 4503.2807.

after interpolating: xx(1) = 4.465627, xd_og(end) = 4503.2807.

After refiltering (so filt_moca function): xx(1) = 5.399023, xx(end) = 4447.172

I really need the refiltering step to smooth my data, but it seems to cut off the last part of my data.

This is my function now, and below a clearer plot that shows the filtering effect.

function traj_intp = interpolate_moca(traj,time,time_s,fs_sens,fs_moca,fco_moca_forICOR)
% Interpolate data
    r = fs_sens/fs_moca; % Sample rate increase factor, must be multiplied with moca frames, and then -9
    traj_intp = nan(size(traj,1),size(traj,2),((r*size(traj,3))-9)); % PUCK EDIT changed r*size(traj,3) into ((r*size(traj,3))-9) 
    traj_og = traj;
    %Fc_x_int = squeeze(traj(Fc_id, 1,:)); % Get arrays for x, y, z trajectories of desired markers, 
    disp([time(1), time(end)]);
    disp([time_s(1), time_s(end)]);
    for mar = 1:size(traj,1)
        % Get trajectory data
        xd_og = squeeze(traj(mar,1,:));
        yd_og = squeeze(traj(mar,2,:));
        zd_og = squeeze(traj(mar,3,:));
        fprintf(" xx start is  %.6f\n", xd_og(1));
        fprintf(" xx end is = %.6f\n", xd_og(end));
        ns = size(xd_og,1); %number of samples
        ins = (r*ns)-9; % interpolated number of samples
        % Corresponding timevector
        t_og = time;
        t_in = time_s; 
        % Find nans
        xxnan = double(isnan(xd_og));
        yynan = double(isnan(yd_og));
        zznan = double(isnan(zd_og));
        % xyznan = [xxnan,yynan,zznan]; Interpolation of gaps
        xxnan_in = interp1(t_og, xxnan,t_in,'linear'); 
        yynan_in = interp1(t_og, yynan,t_in,'linear');
        zznan_in = interp1(t_og, zznan,t_in,'linear');
        xyznan_in = [xxnan_in,yynan_in,zznan_in];
        xyznan_in(xyznan_in<=0.4) = 0;
        xyznan_in(xyznan_in>0.4) = 1;

        % For these, linearly interpolate where the nans were. We will be replacing these by nans again later,
        % but we cannot interpolate with duplicates in the query vector
        xd = fillmissing(xd_og,"pchip");
        yd = fillmissing(yd_og,"pchip");
        zd = fillmissing(zd_og,"pchip");
        % Interpolate
        if numel(t_og) < 2 || sum(~isnan(xd)) < 2
            % nothing to interpolate — just repeat the first value
            xx = repmat(xd(1), numel(t_in), 1);
            yy = repmat(yd(1), numel(t_in), 1);
            zz = repmat(zd(1), numel(t_in), 1);
        else
            xx = interp1(t_og, xd, t_in, "pchip");
            yy = interp1(t_og, yd, t_in, "pchip");
            zz = interp1(t_og, zd, t_in, "pchip");
        end
        
        % Add NaNs back in
        xx((xyznan_in(:,1))==1) = NaN;
        yy((xyznan_in(:,2))==1) = NaN;
        zz((xyznan_in(:,3))==1) = NaN;
        fprintf(" xxi start is  %.6f\n", xx(1));
        fprintf(" xxi end is = %.6f\n", xx(end));
        % Refilter: These will be used to determine the ICOR, so we filter them with a low threshold, otherwise we will get all-over-the-place estimates for the ICOR
        xx = filt_moca(xx, fco_moca_forICOR, fs_sens);
        yy = filt_moca(yy, fco_moca_forICOR, fs_sens);
        zz = filt_moca(zz, fco_moca_forICOR, fs_sens);
        fprintf(" xxf start is  %.6f\n", xx(1));
        fprintf(" xxf end is = %.6f\n", xx(end));
        % Add into complete trajectory (stored variable)
        traj_intp(mar,1:3,:) = [xx;yy;zz];
        
    end
end

function filtered_traj = filt_moca(traj, fco_moca, fs_moca)
% Filters the motion capture data using cutoff frequency
    % Filter inputs: 6th order butterworth filter results in 12th order filter
    [B,A] = butter(6, (fco_moca/(fs_moca/2)), 'low');
    nan_is = isnan(traj);
    not_nan_is = ~isnan(traj);
    x_og_nonan = traj(not_nan_is);
    x_nonan = filtfilt(B,A, x_og_nonan);
    x = traj; x(not_nan_is) = x_nonan; 
    filtered_traj = x';
end

[dpb]

resample

[Meg Noah]

Stineman interpolation is a good smooth operation as well: https://www.mathworks.com/matlabcentral/fileexchange/104195-smooth-operator-stineman-1d-interpolation