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

Mex only on file change

For large Simulink projects with lots of s-function, performing a mex of all files can be time consuming. Leaving it up to the user might result in simulation runs with older mex files because the user might forget to re-mex the updated file. You can automate the process as follows:

• Create a hash list file that consists of file names used by s-functions and corresponding hashes.
• Before simulation start, in InitFcn, 
• Calculate hashes of files used by s-functions and compare them with the values saved in hash list file. Mark files whose hashes are different as "changed".
• If an existing binary mex file date is earlier than any of the dates of the source files used in its build, mark that binary as "changed".
• Mex files marked as "changed".

For hash calculation you can use [status, cmdout] = system(['certutil -hashfile', fileName, ' SHA256']) and parse cmdout via str=splitlines(cmdout); hash=str[2]; to get hash of file.

From Workspace Sample Time

Let's say you have a single Simulink model with plant and controller, and another that just has the plant.

If you save outputs of plant&controller with to workspace blocks, add from workspace blocks to plant only model and run it with outputs of plant&controller, you might see differences between results. In order to have a close match of results, you shouldn't leave the sample time of from workspace blocks as zero, you have to enter them equal to sample time of plant&controller model.