Integration of angular velocity from IMU measurements

26 次查看（过去 30 天）
Sindre Hodneland
Sindre Hodneland 2019-10-2
评论： Sindre Hodneland 2019-10-6
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?
  2 个评论
James Tursa
James Tursa 2019-10-2
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
Sindre Hodneland 2019-10-2
Thanks for reply. I have added the dataset on top of the post if it helps

请先登录，再进行评论。

请先登录，再回答此问题。

采纳的回答

Star Strider
Star Strider 2019-10-2
编辑：Star Strider 2019-10-2
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:
  5 个评论
显示 3更早的评论
Jim Riggs
Jim Riggs 2019-10-3
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
Sindre Hodneland 2019-10-6
Thank you very much for info James and Jim, appreciate it! Will try this with Kalman filter as well

请先登录，再进行评论。

更多回答（0 个）

类别

在 Help Center 和 File Exchange 中查找有关 Online Estimation 的更多信息

标签

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by