Applying derivative and integral on noisy data

Let's say you have a motion with sinusoidal acceleration, i.e. xddot(t) = sin(t). The analytic solution for speed and position will be xdot(t) = -cos(t)+1 (assuming zero initial speed) and x(t) = -sin(t) + t.  Let's say you don't know the analytical expressions of position, speed and acceleration and only have their values at a fixed sample rate. If you want to increase the samples, you can use Matlab's spline function.

If you have position as input and calculate speed and acceleration, if there is any noise in the signal, it will be amplified. If you have acceleration as input and calculate speed and position via integration, noise will be attenuated. Another advantage of using integration instead of derivation is that we can use linear interpolation since when integrating, we don't need smooth second derivatives. This both decreases computational load and lag due to two points in linear interpolation compared to four points in spline. See below Matlab code for an example:

	%Compare calculating speed and acc by taking derivative of position with calculating pos and speed by taking integral of acceleration.
	clc; clear all; close all;
	dt1 = 0.5;
	t = 0:dt1:5*pi;
	xddot = sin(t);
	xdot = -cos(t)+1;
	x = -sin(t)+t+0;
	upscaleFactor = 5; %for a factor of 50, there will be a large initial jump in acceleration due to derivative amplifying initial spline positions being slightly negative
	dt2 = dt1/upscaleFactor;
	t2 = t(1):dt2:t(end);
	noise = 1*randn(1,length(t2))/50;
	xUpsampled = spline(t,x,t2) + noise; %upsample x with t2
	vx = zeros(length(t2),1);
	vx(1)=0;
	for i=2:length(t2)
	vx(i) = (xUpsampled(i)-xUpsampled(i-1))/dt2;
	end
	ax = zeros(length(t2), 1);
	ax(1)=0;
	for i=2:length(t2)
	ax(i) = (vx(i)-vx(i-1))/dt2;
	end
	subplot(3,1,1)
	plot(t, x, '.-'); grid on; hold on
	title('Obtain speed and acc from pos derivative')
	plot(t2, xUpsampled);
	xlabel('t'); ylabel('position'); %xlim([0, 0.2])
	legend('x', 'xUpsampled','Location','southeast')
	subplot(3,1,2)
	plot(t, xdot, '.-'); grid on; hold on
	plot(t2, vx); ylim([-1 2])
	xlabel('t'); ylabel('1st time derivative');%xlim([0, 0.2]);ylim([-0.05, 0.05])
	legend('xdot', 'vx','Location','southeast')
	subplot(3,1,3)
	plot(t, xddot, '.-'); grid on; hold on;
	plot(t2, ax)
	xlabel('t'); ylabel('2nd time derivative');%xlim([0, 0.2])
	legend('xddot', 'ax','Location','southeast')
	%% Obtain velocity and position from xddot integral
	xddotSpline = spline(t,xddot,t2) + noise; %upsample xddot with t2 using spline
	xddotInterp1 = interp1(t,xddot,t2) + noise; %upsample xddot with t2 using interp1
	%xddotUpsampled = xddotSpline;
	xddotUpsampled = xddotInterp1;
	vxInt = zeros(length(t2),1);
	vxInt(1)=xdot(1);
	vxInt(end)=xdot(end);
	for i=2:length(t2)
	vxInt(i) = vxInt(i-1)+xddotUpsampled(i-1)*dt2;
	end
	xInt = zeros(length(t2),1);
	xInt(1)=x(1);
	xInt(end)=x(end);
	for i=2:length(t2)
	xInt(i) = xInt(i-1)+vxInt(i-1)*dt2;
	end
	figure
	subplot(3,1,1)
	plot(t, x, '.-'); grid on; hold on;
	title('Obtain speed and pos from acc derivative')
	plot(t2, xInt)
	xlabel('t'); ylabel('position');
	legend('x', 'xInt','Location','southeast')
	subplot(3,1,2)
	plot(t, xdot, '.-'); grid on; hold on;
	plot(t2, vxInt); ylim([-1 2])
	xlabel('t'); ylabel('2nd time derivative');
	legend('xddot', 'vxInt','Location','southeast')
	subplot(3,1,3)
	plot(t, xddot, '.-'); grid on; hold on;
	plot(t2, xddotInterp1);
	plot(t2, xddotSpline, 'k');
	xlabel('t'); ylabel('2nd time derivative');
	legend('xddot', 'xddotInterp1', 'xddotSpline' ,'Location','southeast')

view raw accToPos.m hosted with ❤ by GitHub