How to Account for Numerical Integration Error?

I'm working on a project using an Arduino Uno and an Adafruit gyroscope. I'm collecting gyroscope data, saving the data to a text file, and importing the data into MATLAB to operate on it. The gyroscope provides me with angular velocity data whereas I am interested in angular position. To get angular position from angular velocity, I use cumtrapz to numerically integrate my data. I also have implemented a three-point weighted average filter to smooth my gyro data as well as averaged a portion of the data to correct for the initial gyro offset.

Everything in my script is working as it should, however, I'm noticing that when I bring my gyro back to the starting position at the end of a movement, my integrated angular position data does not return to zero. However, the angular velocity data with the help of my filter and offset correction does return to zero. This makes it clear that my final offset is a result of the integration process.

From what I've read online so far, it seems that using cumtrapz and numerical integration in general results in a small amount of error. I remember from my Calc I class that using trapezoidal integration is, of course, only an approximation, so some error is to be expected.

My question is how can I take this error into account and correct for it in my integrated angular position data? I've attached some screenshots of my raw angular velocity data, filtered angular velocity data, and my integrated angular position data.

Thank you for your help in advance.

[John D'Errico]

I'm moved Nick's answer into a comment, here as a way to include the code:

clear; clc; close all;

% Import gyroscope data from delimitied text file
gyroData = dlmread('float7.txt',',');

% Convert time from ms to seconds
time = gyroData(:,1) .* .001;   % seconds

% Define raw angular velocities (deg/sec) for each axis
xRaw = gyroData(:,2);
yRaw = gyroData(:,3);
zRaw = gyroData(:,4);

% Filter gyro data using filter function
[xFilt, yFilt, zFilt] = filterGyroData(xRaw,yRaw,zRaw);

% Sample first 100 data values to calculate offset
xOff = xFilt(1,1:100);
yOff = yFilt(1,1:100);
zOff = zFilt(1,1:100);

% Average initial offset values
xAvg = mean(xOff);
yAvg = mean(yOff);
zAvg = mean(zOff);

% Correct filtered data initial offset
xCorr = xFilt - xAvg;
yCorr = yFilt - yAvg;
zCorr = zFilt - zAvg;

% Convert values to radians per second
xDataRad = deg2rad(xCorr);
yDataRad = deg2rad(yCorr);
zDataRad = deg2rad(zCorr);

% Use cum. numerical integration to obtain angular position (radians)
xAngPosRad = cumtrapz(time,xDataRad);
yAngPosRad = cumtrapz(time,yDataRad);
zAngPosRad = cumtrapz(time,zDataRad);

% Convert values back to degrees
xAngPosDeg = rad2deg(xAngPosRad);
yAngPosDeg = rad2deg(yAngPosRad);
zAngPosDeg = rad2deg(zAngPosRad);

% Plot raw angular velocity
figure(1);
plot(time,xRaw,time,yRaw,time,zRaw);
title('Raw Angular Velocity');
xlabel('Time (sec)'); ylabel('Angular Velocity (deg/sec)');
legend('x','y','z');

% Plot filtered angular velocity
figure(2);
plot(time,xCorr,time,yCorr,time,zCorr);
title('Filtered Angular Velocity')
xlabel('Time (sec)'); ylabel('Angular Position (degrees)');
legend('x','y','z');

% Plot filtered angular position
figure(3);
plot(time,xAngPosDeg,time,yAngPosDeg,time,zAngPosDeg);
title('Filtered Angular Position');
xlabel('Time (sec)'); ylabel('Angular Position (degrees)');
legend('x','y','z');
end

Answer 1

[Star Strider]

If you have the Signal Processing Toolbox, use the freqz function to learn what the passband of your weighted-average filter actually is. Don’t assume it’s somehow magickally doing what you want it to do if you’ve not specifically designed its passband to do so.

I would design a bandpass filter that eliminates the d-c component (integrating it may be the source of the baseline wander in your integrated signal) and any high-frequency noise. Do a fft first to determine what frequencies in your data are signals, and what are noise. IIR filters are best for this (in my opinion, at least). There are many ways to design filters in MATLAB, including dfilt and designfilt. My IIR filter design procedure is here: How to design a lowpass filter for ocean wave data in Matlab? Be sure to use the filtfilt function rather than filter, since filtfilt has a maximally-flat phase response, resulting in no phase distortion in your filtered signal.

Signal processing is not difficult with MATLAB, and doing it correctly is always necessary if you want to get the best results from your data.

[NickPK]

It looks like I do have the Signal Processing Toolbox, but I don't have any experience with it. I must admit, I don't have much experience with filtering data in general- I'm learning as I go.

Regardless, I will definitely research how to use it and implement it into my code. Thank you for the link to the IIR filter design. I will look it over and begin to apply it to my data.

Thanks for the help!

[Star Strider]

My pleasure.

If you want to upload/attach a sample of your code (preferably as a .mat file), I can help you get started with the filtering. It’s not difficult, but if you’re new to signal processing, and that’s not the primary focus of your research, some examples can be instructive. The file needs to have your data, and a time vector of sampling times for each data triplet.

[NickPK]

That would be great! I'm not sure exactly what you want, so I've attached copies of my current script, the weighted average filter I made, and a copy of the text file with the gyro data.

Once you run the script, you should have everything you need in the workspace.

[Star Strider]

I apologise for the delay. Filter design can either be straightforward or challenging, depending on the data. Yours was challenging.

You must decide if this is what you want from your data. The upslope at the end turned out to be due to a transient at the end that integrated as a positive constant. It was not easy to figure out how to design the filter to eliminate it. I settled on a Chebyshev Type II design because you have a relatively low sampling frequency of 23 Hz, and I wanted to be certain I could design an effective bandpass filter for your data. (Filter design is necessarily heuristic. I keep working at it until I get a result I like.)

This seems to give a reasonably good result, at least to me:

gyroData = dlmread('NickPK FLOAT7.TXT',',');
time = gyroData(:,1) .* .001;   % seconds
xRaw = gyroData(:,2);
yRaw = gyroData(:,3);
zRaw = gyroData(:,4);

figure(1)
plot(time, gyroData(:,2:end))
xlabel('Time (sec)')
ylabel('Acceleration Amplitude (Unfiltered)')
title('Unfiltered Data')
grid

L = size(gyroData,1);                                                   % Length (samples)
Ts = mean(diff(time));                                                  % Samping Interval (sec)
Fs = 1/Ts;                                                              % Sampling Frequency (Hz)
Fn = Fs/2;                                                              % Nyquist Frequency
F_gyro = fft(gyroData(:,2:end))/L;                                      % Normalised Fourier Transform
Fv = linspace(0, 1, fix(L/2)+1)*Fn;                                     % Frequency Vector
Iv = 1:length(Fv);                                                      % Index Vector

figure(2)
plot(Fv, abs(F_gyro(Iv,:))*2)                                           % Plot Fourier Transform
grid
xlabel('Frequency (Hz)')
ylabel('Acceleration Amplitude (Unfiltered)')
title('Fourier Transform Of Unfiltered Data')
axis([0  1    ylim])

Wp = [0.05  0.50]/Fn;                                                   % Passband (Normalised)
Ws = [0.01  0.70]/Fn;                                                   % Stopband (Normalised)
Rp =  5;                                                                % Passband Ripple (dB)
Rs = 25;                                                                % Stopband Ripple (dB)
[n,Ws] = cheb2ord(Wp,Ws,Rp,Rs);                                         % Chebyshev Type II Filter Order
[b,a] = cheby2(n,Rs,Ws,'bandpass');                                     % Cnebyshev Type II Transfer Function Coefficients
[sos,g] = tf2sos(b,a);                                                  % Second-Order-Section For Stability

figure(3)
freqz(sos, 2^14, Fs)                                                    % Bode Plot Of Filter

gyroDataFilt = filtfilt(sos, g, gyroData(:,2:end));                     % Filter Signal

figure(4)
plot(time, gyroDataFilt)
xlabel('Time (sec)')
ylabel('Acceleration Amplitude (Filtered)')
title('Filtered Acceleration Data')
grid

gyroDataFiltInt = cumtrapz(time, gyroDataFilt);

figure(5)
plot(time, gyroDataFiltInt)
xlabel('Time (sec)')
ylabel('Velocity Amplitude')
title('Velocity (Integrated Filtered Acceleration Data)')
grid

I comment-documented it as well as I could. If you have any questions, I will do my best to answer them.

[NickPK]

Absolutely no need to apologize! You've done so much more than I could have asked for. Thank you so much! I'm going to look over what you've given me. If I have any questions, I will be sure to ask you.

[Star Strider]

As always, my pleasure!

I’ll be here!

[NickPK]

So I've looked over your code and have been running some tests. Did you design this filter to be used specifically and only with the original data I gave you? I ask because with my particular project, the gyro data that's going to be plotted is going to be different every single time. Different movements are going to be recorded with the gyro for different amounts of time. Specific things aren't going to change like the sampling frequency, but the collected data is.

I ask because I've been doing some tests with different sets of data and have been getting really odd results after the data gets filtered. I've attached a screenshot of one of the test runs. This test was intended to be the same movement (a 90 degree rotation along the x-axis) as was recorded with the original data file I gave you, only this time, of course, different data. The first one (NewFilt.png) shows the data plotted using your filter. As you can see, a lot of weirdness ends up happening. The second one (OriginalFilt.png) is a comparison of the same data being plotted using my original filter function and offset correction.

[Star Strider]

I designed it specifically for the data you posted, to eliminate the upslope at the end and the transient that caused it. It should work for other data, but that assumes a similar frequency content in all your data.

I have no idea why it’s giving such results with the new fit. The filtfilt function by default operates along columns of data in a matrix (as does everything in MATLAB I’m aware of), the reason I was able to pass it the matrix and get an appropriate matrix out.

I don’t have the original data for ‘new fit’ so I can’t say for sure, but it almost looks as though you’re giving it the transpose of your data, so it’s operating along the wrong dimension. The filter has no idea what you’re giving it. It just does what you tell it to do.

See if transposing the matrix before filtering it solves the problem. That’s the only solution I can think of.

[NickPK]

I notice in the new data that for some reason my sampling frequency changes from from 23 Hz in the original data (as you mentioned) to 40 Hz. I have no idea why there is an increase like that; the Arduino code did not change at all. Do you think this could be the reason I'm seeing such odd results?

[Star Strider]

Yes. Discrete filters are designed for specific sampling frequencies. The frequency characteristics of the filter should not change (unless the frequency content of the signal changes), but you will have to recalculate them with the new sampling frequency. My code actually designs the filters using the same frequency characteristics but changes them for the sampling frequency, so different sampling frequencies shouldn’t be the problem, unless you’re using the same filter and not recalculating it each time.

I would have to have the original data to see what the problem could be. The filters themselves should not introduce the sort of behaviour you’re seeing. The results should be essentially the same as with the data you originally posted.

[NickPK]

I've collected and attached some data samples for you to have a look at. Unfortunately, I've reached my daily upload limit (didn't know what was a thing) and can't upload both files directly. Here is a link to a Dropbox folder that contains the data files; you should be able to download them.

Anyways, it looks like the sampling frequency changes because the time step in between my data points changes each time. Because of this, when you calculate the average difference between time values, each set of data is going to be different.

I also notice that the larger the sketch I upload to my Arduino board, the lower the frequency between data points (I assume because it takes the board longer to loop through each time if I program it to do more than one thing). Whereas if I keep the sketch small and only have the board collect data and save it, the frequency increases. This seems like the root of the issue.

The first data file (but2.txt) is data I collected using the same movement as the original data (float7.txt). This was collected after running the larger Arduino sketch, so the frequency is lower (around 26 Hz). The second file is the same movement, only it was collected using the smaller Arduino sketch. The frequency for that file comes out much higher (around 46 Hz).

[Star Strider]

I discovered that your data aren’t regularly sampled, so I added an interpolation step in this version to produce a regularly-sampled signal, and redesigned the filter, since the frequency content is different. The low sampling frequency continues to be a problem, so I decided to use the frequency vector from the Fourier transform to define the low edge of the passband, because otherwise it allows a very low frequency baseline wander to appear in the filtered signal. Maybe that will make it adaptable for your other files. Most of the rest of the code is unchanged.

New version:

gyroData = dlmread('NickPK BUT2.TXT',',');
time = gyroData(:,1) .* .001;   % seconds
xRaw = gyroData(:,2);
yRaw = gyroData(:,3);
zRaw = gyroData(:,4);

ti = linspace(min(time), max(time), size(time,1));                      % Interpolation Vector
gyroData = interp1(time, gyroData, ti);                                 % Interpolate Data
time = ti;                                                              % Redefin ‘time’ 

figure(1)
plot(time, gyroData(:,2:end))
xlabel('Time (sec)')
ylabel('Acceleration Amplitude (Unfiltered)')
title('Unfiltered Data')
grid

Qt = [mean(diff(time)) std(diff(time))];
L = size(gyroData,1);                                                   % Length (samples)
Ts = mean(diff(time));                                                  % Samping Interval (sec)
Fs = 1/Ts;                                                              % Sampling Frequency (Hz)
Fn = Fs/2;                                                              % Nyquist Frequency
F_gyro = fft(gyroData(:,2:end))/L;                                      % Normalised Fourier Transform
Fv = linspace(0, 1, fix(L/2)+1)*Fn;                                     % Frequency Vector
Iv = 1:length(Fv);                                                      % Index Vector

figure(2)
plot(Fv, abs(F_gyro(Iv,:))*2)                                           % Plot Fourier Transform
grid
xlabel('Frequency (Hz)')
ylabel('Acceleration Amplitude (Unfiltered)')
title('Fourier Transform Of Unfiltered Data')
axis([0  8    ylim])

Wp = [Fv(3)*0.8  9.20]/Fn;                                              % Passband (Normalised)
Ws = [Fv(2)*0.1  9.70]/Fn;                                              % Stopband (Normalised)
Rp =  5;                                                                % Passband Ripple (dB)
Rs = 25;                                                                % Stopband Ripple (dB)
[n,Ws] = cheb2ord(Wp,Ws,Rp,Rs);                                         % Chebyshev Type II Filter Order
[b,a] = cheby2(n,Rs,Ws,'bandpass');                                     % Cnebyshev Type II Transfer Function Coefficients
[sos,g] = tf2sos(b,a);                                                  % Second-Order-Section For Stability

figure(3)
freqz(sos, 2^14, Fs)                                                    % Bode Plot Of Filter

gyroDataFilt = filtfilt(sos, g, gyroData(:,2:end));                     % Filter Signal

figure(4)
plot(time, gyroDataFilt)
xlabel('Time (sec)')
ylabel('Acceleration Amplitude (Filtered)')
title('Filtered Acceleration Data')
grid

gyroDataFiltInt = cumtrapz(time, gyroDataFilt);

figure(5)
plot(time, gyroDataFiltInt)
xlabel('Time (sec)')
ylabel('Velocity Amplitude')
title('Velocity (Integrated Filtered Acceleration Data)')
grid

I can’t download directly from Dropbox, so I copy what I can read to a ‘notepad’ file and create it from there. One extra step, but no problem.

I’ll take a look at the other file in a few minutes. If we have to design a new filter for each data set, this could be interesting!

--------------------------------------------------------------------------------------------------------------------------------

EDIT — There is a serious problem with the ‘NickPK NOBUT1.TXT’ file. If you plot it in the time domain and look at it closely, you’ll see that it contains overlapping times in two different sections of the file, so the ‘zig-zag’ is in the original data.

That can be fixed with a bit of creative coding:

gyroData = dlmread('NickPK NOBUT1.TXT',',');
time = gyroData(:,1) .* .001;   % seconds
xRaw = gyroData(:,2);
yRaw = gyroData(:,3);
zRaw = gyroData(:,4);

brk = find(diff([0; time]) <0 );                                        % Find ‘break’ In Time Vector
time(brk:end) = time(brk:end) + time(brk-1);                            % Add ‘Broken’ End To End Of Time Vector
ti = linspace(min(time), max(time), size(time,1));                      % Interpolation Vector
gyroData = interp1(time, gyroData, ti);                                 % Interpolate Data
time = ti;                                                              % Redefine ‘time’ 

figure(1)
plot(time, gyroData(:,2:end))
xlabel('Time (sec)')
ylabel('Acceleration Amplitude (Unfiltered)')
title('Unfiltered Data')
grid
axis([xlim    -10  10])

Qt = [mean(diff(time)) std(diff(time))];
L = size(gyroData,1);                                                   % Length (samples)
Ts = mean(diff(time));                                                  % Samping Interval (sec)
Fs = 1/Ts;                                                              % Sampling Frequency (Hz)
Fn = Fs/2;                                                              % Nyquist Frequency
F_gyro = fft(gyroData(:,2:end))/L;                                      % Normalised Fourier Transform
Fv = linspace(0, 1, fix(L/2)+1)*Fn;                                     % Frequency Vector
Iv = 1:length(Fv);                                                      % Index Vector

figure(2)
plot(Fv, abs(F_gyro(Iv,:))*2)                                           % Plot Fourier Transform
grid
xlabel('Frequency (Hz)')
ylabel('Acceleration Amplitude (Unfiltered)')
title('Fourier Transform Of Unfiltered Data')
axis([0  8    ylim])

Wp = [Fv(3)*0.2  9.20]/Fn;                                              % Passband (Normalised)
Ws = [Fv(2)*0.3  9.70]/Fn;                                              % Stopband (Normalised)
Rp =  5;                                                                % Passband Ripple (dB)
Rs = 25;                                                                % Stopband Ripple (dB)
[n,Ws] = cheb2ord(Wp,Ws,Rp,Rs);                                         % Chebyshev Type II Filter Order
[b,a] = cheby2(n,Rs,Ws,'bandpass');                                     % Cnebyshev Type II Transfer Function Coefficients
[sos,g] = tf2sos(b,a);                                                  % Second-Order-Section For Stability

figure(3)
freqz(sos, 2^14, Fs)                                                    % Bode Plot Of Filter

gyroDataFilt = filtfilt(sos, g, gyroData(:,2:end));                     % Filter Signal

figure(4)
plot(time, gyroDataFilt)
xlabel('Time (sec)')
ylabel('Acceleration Amplitude (Filtered)')
title('Filtered Acceleration Data')
grid

gyroDataFiltInt = cumtrapz(time, gyroDataFilt);

figure(5)
plot(time, gyroDataFiltInt)
xlabel('Time (sec)')
ylabel('Velocity Amplitude')
title('Velocity (Integrated Filtered Acceleration Data)')
grid

That works, and seems to give a decent result. You may want to tweak the filter to get a better result. It is difficult for me to entirely remove the baseline variation in the integrated signal, probably because of the effects both of the low sampling frequency and the integration. Note that the upper passband is now unchanged, so that may work for your other data. I did my best to make this as adaptive as I can.

Let me know if you have any further questions or problems.

[NickPK]

Thank you so much! You have really gone above and beyond the help I expected to receive. I'll take some time later to analyze the new code and do some tests with it. I apologize for the faulty data. That's the first time that's happened.

It's worth mentioning that I'm in the process of testing a new sensor. Specifically the one found here. In case you were interested, the original gyro I've been using is this one.

I've just finished programming the new IMU to log and save data and so far it seems that the new sensor is much more accurate from the start. In fact, from what I'm seeing, it doesn't appear to have any initial drift in the angular velocity. However, I'm still seeing final offset in data after integrating.

I'm going to continue to play around and test this new sensor as well as the old one. I'll post questions as I come across them.

Also, why couldn't you download directly from Dropbox? Was it something on my end?

[Star Strider]

As always, my pleasure!

I enjoy helping with ‘real world’ data analysis problems, especially when they overlap into an area of my interests.

Not a problem on your end. I just have no reason to create a Dropbox account. It worked regardless, so no problems.

I’ll take a look at the sensor information you linked to. I used to do a fair amount of reading from USB sensors in MATLAB but haven’t needed to in the past few years. It’s probably something I would enjoy getting into again.

Answer 2

[John D'Errico]

In order to compute the error of a numerical integration as an approximation, you would need to know the true function, and the true integral.

The fact is, you don't KNOW the true function. You don't KNOW the true integral!

So you cannot account for the unknowable.

That being said, I don't KNOW that you don't have a bug in your code.

[NickPK]

Thanks for the response! I was expecting that to be the case. I will post my current script so you can look over it.

[John D'Errico]

Please post comments as a COMMENT, not as an answer. Or simply attach it to your question as an m-file.

As well, why did you feel the need to comment out the entire script? I had to remove all of the spurious percent signs that you put in.

[NickPK]

Sorry about that. I'm trying to keep up with responses in between my classes and didn't notice.

As far as the percent signs, I wasn't sure how to the include code on this forum; it's a little different from others I've used with the if statement that it pulls up when you press the code button. I apologize for the inconvenience. In the future I'll just attach m-files.

[John D'Errico]

The trick with the code button is to past in the code, THEN select it, THEN click on the "{} Code" button. If you click on the code button with nothign selected, then is adds a spurious if true conditional, I assume to show you where you need to include your code. It just confuses people though.

All the "{} Code" button really does is insert two spaces before every selected line. Lines indented with two spaces at the start are recognized as lines of code.

So this is interpreted as a line of code, and highlighted as such.

And this is not.

The only difference was I indented the first line above.

[NickPK]

Thank you for that. It didn't occur to me that I needed to do it in that order. Now I know.

[John D'Errico]

Ok, I looked at your code. My first inclination is that you filtered it, which must of course introduce some error too. I'll need to look more carefully at what you have done.