# Correct filter to find angular velocity from a rotating accelerometer?

I have researched a bit into DSP filters for the problem of finding the angular velocity from X/Y acceleration measured by a rotating accelerometer. Essentially I need more help in picking the best type of filter.

An example of the type of data I can measure can be seen in this image (ignore the y-axis as values are scaled and offset):

The model for the ideal signal without noise and with a constant angular velocity (w) is fairly simple (Scilab code):

// Signal Model
DeltaT = 0.01;          // 100Hz sampling
w = (50 / 60) * 2*%pi;  // 50 RPM -> radians/sec
L = 8/100;              // 8 cm -> meters
g = 9.81;               // Earth gravity in ms^-2

N = 1000; // Number of samples
for i = 1:N
x_val(i) = g * cos(w * (i-1)*DeltaT);
y_val(i) = g * sin(w * (i-1)*DeltaT) + w*w * L;
end


Update: Here we ignore the linear acceleration/brake of the entire system along the road. This becomes a (significant) noise adder.

Shown here as a plot

Sample rate is 100 Hz. The accelerometer is mounted on a crank arm of a bike (between the crank and the pedal). The Y-axis of the accelerometer points away from the center of the rotation and is mounted 8 cm away from the center, so this sine wave is offset a bit by the centripetal acceleration (red curve). The ideal result/filter output here would be a flat line with the angular velocity (w).

The noise can change any second. The angular velocity can change any second – both slowly and abruptly. The quest is to find the best estimate for the angular velocity at any time.

My first attempts involved using a running average filter to reduce noise and digitizing with hysteresis to find two points on the rotation. This works, but is obviously too crude as we want to estimate the velocity at every sample point. One such attempt is shown here:

Red and blue as before (ignore scale/offset). Black is the output in RPM. As can be seen this is VERY crude. Lot's of things can be improved (curve does not go to zero when the accelerometer is clearly not moving; staircase effect from only calculating angular velocity once per revolution etc.)

I know DSP filtering techniques like Kalman (probably not good, as this is not a linear problem?), Total Variation (probably not good as the piecewise constant regions are more ramps than plateaus?) etc. can do wonders.

But what exactly is a good/"the best" method to use for this type of signals?

Trying to run real data though Peter K's filter (see answer below).

I can create a measured version of "z" like this (scilab code):

 // Read in the measured accelerometer data from datalogger
ACCM = fscanfMat("C:\_work\2et\pedal-acc\LOG05.txt")
[nr, nc] = size (ACCM);
x_avg = sum(ACCM(:, 1))/nr; // Use average as 0g
xacc_meas = g * (ACCM(:, 1) - x_avg) / 93; // 300mV/g; 3.3V~1024 ADC steps
yacc_meas = g * (ACCM(:, 2) - x_avg) / 93;

// Use an intersting section as input to the filter
first_sample = 60000;
xacc_meas = xacc_meas(first_sample:first_sample+N-1);
yacc_meas = yacc_meas(first_sample:first_sample+N-1);

// Construct a measured version of the "z" vector
z_meas = zeros(2, N);
z_meas(1, :) = matrix(xacc_meas, 1, N); // x-acceleration
z_meas(2, :) = matrix(yacc_meas, 1, N); // y-acceleration


Comparing the measured data to the (fake) "z" values in Peter K's simulation:

Note there is no time correlation - just looking at how data "looks". The noise factor sig_v needs to be increased to about 4-5 for similar noise level.

I change the two noise factors like this (assuming this is how we "tune" the filter):

 sig_w = 0.0001; // Was: 0.00001;
sig_v = 4;      // Was: 1;


And construct a very rough reference using my previous crude method:

 // Make a rough reference from measured data called "w_ref"
// Functions not shown - ask if you want to see them
xacc_temp = running_avgws(xacc_meas, 15);
xacc_temp = running_avgws(xacc_temp, 15);
xacc_hyst = hysthresis(xacc_temp, 2.5);


I can now run the EKF on real measured data and get the following results:

and

Clearly not there yet. I expected the middle plot to match somewhat (blue is the output of the filter for angular velocity and red is my crude reference - note that the middle section really should be zero).

I can't seem to find any value of sig_w that will make it "lock". What have I done wrong?

UPDATE:

With the changes proposed it works, but I am not impressed. Look at a fairly simple stretch of real world samples and how the EKF works on this:

The green shows the samples after two times running average with a window size of 15. That looks so nice and clean. The red curve shows my very crude algorithm doing a reasonable fine job by just looking at the zero-crossings. The blue is the estimate by the EKF.

I was expecting the EKF would come to a much cleaner result as it has built-in knowledge about the physical model with sine waves and all. Is this really all we can get out of it? Or is there something wrong in the way we do it?

Note: I accept that there may be a systematic variation in the angular velocity per rotation that I have a hard time verifying.

PS: I have ignored this line of the code:

 xhatkkm1(:,1) = x(:,1); // Seeding with an initial condition (cheating)

• For my understanding: in order to obtain the angular velocity you would have to differentiate the blue and red curve with respect to time, right? Can you combine the two results to get a better estiamte of the angular velocity? – Deve Nov 11 '13 at 9:37
• No not really. If you differentiate sin(x) or cos(x) you just get the other one (and maybe a polarity change). But you can solve the two model equations to get from x/y acceleration to angular velocity (w) - not too difficult. I am thinking one would use both signals and the ideal model as the predictor of the next samples (x and y acceleration) in the filter to remove the noise. – Rolf Ostergaard Nov 11 '13 at 10:01
• Sure, my fault. You would have to differentiate the phase of the blue/red curve, of course. After removing the DC bias of $y$ you could get the phase by $\arctan(y/x)$. But my question is: are you asking for a filter that provides you with what you've shown ideally in your second plot or one that yields the angular velocity directly? – Deve Nov 11 '13 at 10:28
• The output of interest is really the angular velocity (w). Best estimate for each sample (some pipeline lag is okay, but not more than say half a second or 50 samples). I assume that the filter may have to estimate the ideal x/y waveforms as part of the process - but not really needed. Notice that the difficult part may be that the angular velocity (w) is not constant as in the ideal example, but ramps up and down + can jump to zero at any time. – Rolf Ostergaard Nov 11 '13 at 10:35
• @Deve Thanks for taking interest in this. I updated the Q with an example of the (poor) output of my first filter attempt (very crude). I hope that gives you an idea. – Rolf Ostergaard Nov 11 '13 at 10:58

As you say, the Kalman filter is probably not the best due to the nonlinear nature of the dynamics. However this post over on a sister SE site suggests that the next step might just be an Extended Kalman Filter, which is really just a Kalman filter applied to the first order Taylor series expansion of the nonlinear dynamics.

I cannot say how well this will work, but let's see if we can write down the equations for the signal model.

Let's use the states as: $$x_k = [ \theta_k\ \omega_k \ a_k ]^T$$ and let's use the state update equation: $$x_{k+1} = \left[ \begin{array}{ccc} 1 & 1 & 0\\ 0 & 1 & 1\\ 0 & 0 & 1 \end{array} \right] x_k + w_k = \left[ \begin{array}{c} \theta_k + \omega_k\\ \omega_k + a_k\\ a_k \end{array} \right] + w_k$$ where $w_k$ is the process noise of covariance $Q$.

The output equation could then be: $$z_k = \left[ \begin{array}{cc} g \cos(\theta_k) \\ g \sin(\theta_k) + \omega_k^2 L \end{array} \right] + v_k$$ where $v_k$ is the measurement noise with covariance $R$.

I've probably made some mistake in the code below (feel free to correct it or at least point out the problem!), but this signal model seems to generate a reasonable EKF.

The code assumes a reasonably constant omega, except for a quarter of the way through until half-way through, where I'm just setting it to zero arbitrarily.

The EKF estimate and the actual estimate are shown here:

And the state estimates for $\theta$, $\omega$ and $a$ do not seem too bad:

UPDATE

OK, there are FOUR problems:

1) The model that I used was rotating the wrong way compared with your data. This means I had to change the two functions:

function h = output_equation(theta, omega, g, L)
h = [g*cos(theta); -g*sin(theta) + omega.*omega*L];
endfunction


and

function h = cap_hk(theta, omega, g,L)
h = [-g*sin(theta) 0 0; -g*cos(theta) 2*omega*L 0]'
endfunction


Note the minus signs on the $\sin$ term in the first one and the $\cos$ term in the second.

2) The second problem is that the w_ref variable and the xhatkk(2,:) variable are on different scales. I had to scale xhatkk(2,:) by (one on) DeltaT to get them to align.

3) The third issue is that xhatkk(2,:) is not very smooth. If I run the running_avgws function over it a couple of times, things look a little better.

4) I had to revert to sig_w = 0.00001; to get lock to happen.

The plot below shows the w_ref value in red. The top plot is the raw xhatkk(2,:) value. The bottom plot shows the smoothed version.

Code to generate the plot

sm = running_avgws(xhatkk(2,:)'/DeltaT,15);
sm2 = running_avgws(sm,15);

figure(3);
clf;
subplot(211)
plot(w_ref,'r');
plot(xhatkk(2,:)/DeltaT,'k:');
mtlb_axis([0 1000 0 10]);
subplot(212)
plot(w_ref,'r');
plot(sm2,'k:');
mtlb_axis([0 1000 0 10]);


Scilab code to generate plots

// Constants
DeltaT = 0.01;          // 100Hz sampling
w = (50 / 60) * 2*%pi;  // 50 RPM -> radians/sec
L = 8/100;              // 8 cm -> meters
g = 9.81;               // Earth gravity in ms^-2
N = 1000;               // Number of samples

sig_w = 0.00001;
sig_v = 1;
ww = sig_w*rand(3,N,'normal');
vv = sig_v*rand(2,N,'normal');

x = zeros(3,N);
x(:,1) = [0,w*DeltaT,0]';

z = zeros(2,N);

F = [1 1 0; 0 1 1; 0 0 1];

// All nonlinearities are in the output equation
function h = output_equation(theta, omega, g, L)
h = [g*cos(theta); g*sin(theta) + omega.*omega*L];
endfunction

for i=2:N,
x(:,i) = F*x(:,i-1) + ww(:,i);
if (i>N/4) & (i<N/2) then
x(2,i) = 0;
end
th = x(1,i);
om = x(2,i);
z(:,i) = output_equation(th, om, g, L) + vv(:,i);
end

// Extended Kalman Filter
Q = sig_w*eye(3,3);
R = sig_v*eye(2,2);

G = eye(3,3);

function h = cap_hk(theta, omega, g,L)
h = [-g*sin(theta) 0 0; g*cos(theta) 2*omega*L 0]'
endfunction

F_k = F;

Sigma_kkm1 = 1*eye(3,3);
xhatkk = zeros(3,N);
xhatkkm1 = zeros(3,N);
xhatkkm1(:,1) = x(:,1);

HH = [];

for i=1:N,
H_k = cap_hk(xhatkkm1(1,i),xhatkkm1(2,i),g,L);
HH = [HH; H_k];
Omega_k = H_k'*Sigma_kkm1*H_k + R;
L_k = Sigma_kkm1*H_k*pinv(Omega_k);
xhatkk(:,i) = xhatkkm1(:,i) + L_k*(z(:,i) - output_equation(xhatkkm1(1,i),xhatkkm1(2,i),g,L));
xhatkkm1(:,i+1) = F_k*xhatkk(:,i);
Sigma_kk = Sigma_kkm1 - Sigma_kkm1*H_k*pinv(H_k'*Sigma_kkm1*H_k + R)*H_k'*Sigma_kkm1;
Sigma_kkm1 = F_k*Sigma_kk*F_k' + G*Q*G';
end

XXX = output_equation(xhatkkm1(1,:),xhatkkm1(2,:),g,L);

figure(1);
clf
subplot(311)
plot(x(1,:),'r')
plot(xhatkk(1,:),':')
title('Cumulative phase: truth (red) and estimate (blue)')
subplot(312)
plot(x(2,:),'r')
plot(xhatkk(2,:),':')
title('Frequency: truth (red) and estimate (blue)')
subplot(313);
plot(x(3,:),'r')
plot(xhatkk(3,:),':')
title('Acceleration: truth (red) and estimate (blue)')

figure(2);
clf;
subplot(211)
plot(z')
plot(XXX','.')
title('Measurements (line) and estimages (dots)');
subplot(212)
plot(z'-XXX(:,1:1000)');
title('Error between estimate and measurement')

• Thanks for showing interest in this. I am concerned about two things: 1) The two "modes" of coasting and applying torque. When coasting W drops to zero at once (no inertia). When applying torque it's more smooth. This can not really be included in the Kalman predictor so it will have high uncertainty. 2) I ignored the linear acceleration of the bike on the road which has the same predictor problem. Do you still think the EKF is the best bet? – Rolf Ostergaard Nov 11 '13 at 20:03
• @RolfOstergaard: I think the EKF should work fine. See my edits. I've probably not done the signal model quite correct, but it seems to hold together reasonably well, even with arbitrary fixing of the frequency here. – Peter K. Nov 12 '13 at 2:41
• Wow what an amazing answer. You will have to increase sig_v to about 4-5 to match real measured data. Next thing would be to run real data through it. I can create a measured version of "z" no problem, but I may need help in creating a measured version of "x". – Rolf Ostergaard Nov 12 '13 at 10:09
• The scilab code put there has two parts: the signal model (fake signal) and the EKF. Your measurements are the $z$ in the equation. Pass this into the EKF (with more appropriate values for sig_v and sig_w as you suggest) and the measured version of x will be in xhatkk ($\hat{x}_{k|k}$) or xhatkkm1 ($\hat{x}_{k|k-1}$). – Peter K. Nov 12 '13 at 12:40
• Here is a ton of samples (and a copy of my version of the code): dl.dropboxusercontent.com/u/89040139/EKF_1.zip – Rolf Ostergaard Nov 12 '13 at 15:32