Integration of angular velocity from IMU measurements

Hi,

I have gathered some data from an IMU attached to a boat. I want to integrate the angular velocity measurements to get the roll and pitch angles. For this I used this code:

t = AngularVelocity.time;
p = AngularVelocity.x;
q = AngularVelocity.y;
roll = cumtrapz(t,p);
pitch = cumtrapz(t,q);

figure
plot(t,p)
hold on
plot(t,roll)
legend('p','roll')

figure
plot(t,q)
hold on
plot(t,pitch)
legend('q','pitch')

3 first lines are from a struct gathered from the IMU sensor.

The first figure looks fine:

but I dont understand the plot below of the pitch angle. The boat is 5 meters and the speed is around 2 knots, so the pitch angle should go up and down around zero like the angular velocity (q) right? Anyone know why the pitch is increasing like this?

[James Tursa]

What exactly is your data? Delta angles that are sampled at some frequency? What do you get when you simply plot the cumsum( ) of the data?

[Sindre Hodneland]

Thanks for reply. I have added the dataset on top of the post if it helps

Answer 1

[Star Strider]

The pitch angle has a non-zero offset (so it is consistently greater than zero). The integration is integrating this constant as well, creating the upward slope.

The easiest approach would be to use a high-pass filter (with a specific low-frequency cutoff) to eliminate the non-zero baseline, then do the integration. This elimiinates the offset, and the integral should then be relatively flat. (Another approach to fixing it would be to subtract the mean value of the pitch angle from the pitch angle data. I would use the filter.)

EDIT —

The filtering approach —

D = load('AngularVelocity.mat');
AngularVelocity = D.AngularVelocity;
tv = AngularVelocity.time;
pv = AngularVelocity.x;
qv = AngularVelocity.y;

figure
plot(tv, pv)
grid

% Ts = mean(diff(tv));
% Fs = 1/Ts;
[pn,tn] = resample(pv, tv, 125);                                            % Resample To Constant Sampling Interval
[qn,tn] = resample(qv, tv, 125);                                            % Resample To Constant Sampling Interval

Fs = 125;                                                                   % Sampling Frequency
Fn = Fs/2;                                                                  % Nyquist Frequency
L = numel(tn);                                                              % Signal Length
FTpn = fft(pn)/L;                                                           % Normalised Fourier Transform
Fv = linspace(0, 1, fix(L/2)+1)*Fn;                                         % Frequency Vector
Iv = 1:numel(Fv);                                                           % Index Vector

figure
plot(Fv, abs(FTpn(Iv))*2)
grid
xlim([0 5])

Wp = 0.20/Fn;                                                               % Passband Frequency (Normalised)
Ws = 0.50/Fn;                                                               % Stopband Frequency (Normalised)
Rp =  1;                                                                    % Passband Ripple
Rs = 60;                                                                    % Passband Ripple (Attenuation)
[n,Wp] = ellipord(Wp,Ws,Rp,Rs);                                             % Elliptic Order Calculation
[z,p,k] = ellip(n,Rp,Rs,Wp,'high');                                         % Elliptic Filter Design: Zero-Pole-Gain 
[sos,g] = zp2sos(z,p,k);                                                    % Second-Order Section For Stability

figure
freqz(sos, 2^16, Fs)                                                        % Filter Bode Plot

pn_filt = filtfilt(sos,g, pn);                                              % Filter ‘p’
qn_filt = filtfilt(sos,g, qn);                                              % Filter ‘q’

figure
plot(tn, pn_filt)
hold on
plot(tv, pv)
hold off
grid

roll = cumtrapz(tn,pn_filt);
pitch = cumtrapz(tn,qn_filt);
figure
plot(tn,pn_filt)
hold on
plot(tn,roll)
legend('p','roll')
figure
plot(tn,qn_filt)
hold on
plot(tn,pitch)
legend('q','pitch')

produces:

[Sindre Hodneland]

Wow thanks so much! Just curious: will a kalman filter solve it as well?

[Star Strider]

As always, my pleasure!

If I remember correctly, Kalman filters are used for tracking and other estimation applications. I am not certain what application they would have here.

The filter I use here simply filters out the constant and low-frequency parts of the signal so trapz does not integrate them. (I filtered both signals to be certain they shared those spectral charracteristics, as well as the sampling frequency and the resulting length that the resample call created.)

[James Tursa]

Note that this technique is only applicable because you presumably know that the IMU is fixed on a boat and that the boat is not somersaulting but remains somewhat level throughout, etc.

Arbitrarily removing the "constant" part of IMU data is not a good general technique for all problems.

[Jim Riggs]

James Tursa brings up a good point.

The task you are working is called "Strap-Down Navigation", because it employs inertial sensors which are fixed to ("straped down to") the body that is moving. Strap-down navigation is a highly specialized, (very complex) field of numerical methods. The best course on this is given by Paul Savage of Strapdown Analytics Inc. His text book is in 2 volumes (1600 pages!) and deals with the equations and methods to integrate the output of a strapdown IMU to obtain position and attitude, and yes, it employs the use of a Kalman filter. All IMU's have errors, and the Kalman filter is used to estimate and remove the errors. In your case, you are seeing a bias (rather large one) in the pitch rate, which is why the pitch angle is drifting away from zero. But it is also important to recognize that there are many other sources of errors. The Kalman filter works best when it incorporates aditional information about the body motion, such as position and velocity from a GPS reciever. This is what allows the kalman filter to figure out not only the biases in the IMU, but also if it is tilted (i.e. not perfectly aligned with the body).

One way to look at it is that you are acting as a Kalman filter when you employ Star Strider's method, because you are emposing outside knowledge of the motion to bound an error.

For very short periods of time, you can get away with very crude methods like this, but the errors are nonlinear, coupled, so they tend to grow exponentially with time as you continue to integrate.

[Sindre Hodneland]

Thank you very much for info James and Jim, appreciate it! Will try this with Kalman filter as well