Complimentary filter issues

Question

I'm trying to implement the complimentary filter to get Euler angles using accelerometer and gyroscope data. Attached is the MATLAB code that I have along with a data set.

The data corresponds to moving the sensor from 0-90 degrees while attached to a goniometer. The sensor has an inbuilt algorithm that outputs Euler angles too and I'm trying to test the accuracy of this algorithm as it tends to overshoot the angle estimates.

The problem with the complimentary filter is that the angles move between (around) negative 40 and positive 40 degrees instead of changing between (around) 0-90 degrees.

Can anyone please point out what is wrong and why the complimentary filter isn't working well.

clc;
clear all;
close all;
M=importdata('Multiple_Sensors_747000.csv');
A=M.data;
[m n]=size(A);
a=1;
t=m/60;
angle=0;

for i=1:3723
    Acc(a,:)=M.data(i,6:8); % Reading Accelerometer data
    R_norm(a)=sqrt(Acc(a,1)^2+Acc(a,2)^2+Acc(a,3)^2); % Normalized accelerometer 
    Racc_norm1(a,1)=acos(Acc(a,1)/R_norm(a));         % Angle from accelerometer in X
    Racc_norm1(a,2)=acos(Acc(a,2)/R_norm(a));         % Angle from accelerometer in Y
    Racc_norm1(a,3)=acos(Acc(a,3)/R_norm(a));         % Angle from accelerometer in Z

    Gyroscope(a,:)=M.data(i,3:5);                     % Reading gyroscope data
    gyro(a,:)=Gyroscope(a,:)*(1/t);                   % Integrating gyroscope data
    r(a,1)=sum(gyro(:,1));                            % Angle from gyroscope data
    r(a,2)=sum(gyro(:,2));
    r(a,3)=sum(gyro(:,3));
    angle=0.99*(angle+(gyro(a,:)))+(0.01*Racc_norm1(a,:)); % Complimentary filter equation
    ang(a,:)=angle;
    a=a+1;
end

Answer 1

There are quite a few things wrong here. I'll split them into two sections: technical errors, and coding warnings.

Technical Errors:

1. You are not calculating your angles from accelerometer readings correctly. Consider the arguments in general - they are the normalized accelerometer readings on each axis. You then take the inverse cosine of these. So, if the accelerometer is sitting flat on a table, a_z = -1 (or 1, depending on the polarity/handedness of your sensor), and a_x and a_y are zero. You then take the inverse cosine of those values, to find that the angle of rotation about the x and y axes are +90 degrees and the angle of rotation about the z axis is zero. This doesn't make sense. Consider instead using the equations found here:

$$ \theta_x = \arctan{\left(\frac{X}{\sqrt{Y^2 + Z^2}}\right)} \\ \theta_y = \arctan{\left(\frac{Y}{\sqrt{X^2 + Z^2}}\right)} \\ $$

Here you can see that, if the accelerometer is reading 0 on the x/y axes and +/-1 on the z-axis, that there is no rotation about the x or y axes. This makes sense.

2. You are not using the gyroscope angles. The gyroscope outputs an angular velocity, not an angular position. In order to get the angular position, you need to numerically integrate the gyroscope readings. You seem to have this in your code: r = sum(Gyroscope)*(1/T);, but then you fail to use those angles in the complimentary filter. Instead you go back to the raw gyro outputs.

3. Your complimentary filter isn't a complimentary filter. A complimentary filter is like a lag filter. Each have the form: y = (k)*a + (1-k)*b;. In the complimentary filter, a and b are two different signals, and k is like a "blend" factor, where you take k% of one signal and add it to 1-k% of the other signal. You end up with 100% of signal strength if you do this. In a lag filter, a is the current sample and b is the previous output of the lag filter. The k and 1-k argument still stands, though, and you still wind up with 100% of signal strength. I'll split what you're doing into parts to try to make it more clear what you're doing:

You have:

angle=0.99*(angle+(gyro(a,:)))+(0.01*Racc_norm1(a,:));
ang(a,:)=angle;

Splitting this up a little, you could rephrase it by pulling the angle term out of its own definition and calling a different variable - prevAngle. Factor the 0.99 that it's multiplied by as well and you get:

prevAngle = angle;
angle = 0.99*(gyro(a,:))+(0.01*Racc_norm1(a,:));
ang(a,:) = 0.99*prevAngle + angle;

So here it's like you've got a complimentary filter in the form of:

k = 0.99;
angle = k*gyro(a,:) + (1-k)*Racc_norm1(a,:);

but, your lag filter is broken, because after that line you have:

k2 = 0.99;
ang(a,:) = k2*prevAngle + (k2+(1-k2))*angle;

which is the same another way of saying what you have, which is:

ang(a,:) = k2*prevAngle + (1)*angle;

Hopefully here you can see that, while your complimentary filter is setup okay and you retain 100% signal strength, your lag filter is broken and each sample increases in magnitude by 1.99. You should be doing k2*prevAngle + (1-k2)*angle, but instead of adding (1-k2)*angle, where (1-k2) is 0.01, you're instead adding 1*angle.

So, in summary, you are getting gibberish from the accelerometers, and combining that with gyroscope velocities with a broken filter.

Coding Warnings:

I'm really not trying to poke you in the eye/salt in the wound at this point, I'm just pointing out some flaws and hopefully giving some guidance that should speed up the work I'd imagine you'll be doing in the future. (Combination of IMU filtering and Matlab all sounds like a senior/grad project).

1. Don't use magic numbers. Your for loop runs to 3273. What is that? Why that specific number? You seem to have a grasp of the size() function. I'll point out that you can call size() two different ways - as you have, which is [m,n] = size(data), or another way - m = size(data,1) and n = size(data,2). You never use n, so I'm assuming all you care about is the number of rows. I generally get the number of samples/rows with a one-line call like: nSamples = size(data,1);. Then I can use nSamples instead of magic numbers in all other places and I don't need to go back and edit the code for different data sets. Manually re-coding for every data set is hazardous to your sanity and can give you lots of grief when you forget to change all of the magic numbers and then switch data sets.

2. Don't manually increment anything. You declare a=1; and then use the a=a+1; at the end of your for loop, using a everywhere you could otherwise use i, your loop variable.

3. Use descriptive names. I generally avoid i and similar, even for loop variables. First, i without declaration is 1i, the imaginary number in Matlab. Second, it gets a little tedious/confusing in nested loops to go i/j or i/ii or whatever. As with note 1, I declare my samples to be nSamples, and my for loops are generally for currentSample = 1:nSamples. This can blow up column widths, but the code is so much easier to read and the loop variables are so much easier to manage that I figure it's worth it. One loop can loop through your currentSample and your other loop can then run through currentColumn or whatever.

4. Vectorize. Vectorizing will always speed up your code. Matlab is terrible, terrible at doing any kind of looping. They have algorithms that run vector operations faster than if you looped over it. In C languages there shouldn't be much of a difference, but Matlab isn't C and it doesn't run in real time. I clocked your code at about 140ms to execute when I made a fake data set of size randn(3723,8) and used that for your code. When I used vector operations I got that down to 8ms, and that includes the loop required to do the lag filter. If you did just the complimentary filter, then you can take that loop out and the time drops to 0.8ms. This means that, even though I'm still running a loop, my vectorized version is 16x faster than your unvectorized version. Without the loop it's 160 times faster. I wanted to harp on vectorizing a little because I had simulations as a grad student that ran 24 hours per day for up to 9 days. Every millisecond matters when you're on that time scale. You might not ever do anything that intense, but you'll almost certainly work on larger datasets at some point. This looks like a minute of data.

Not only is it faster, but it's also easier to look at. Consider the following, from your code:

for i=1:3723
    Acc(a,:)=M.data(i,6:8); % Reading Accelerometer data
    R_norm(a)=sqrt(Acc(a,1)^2+Acc(a,2)^2+Acc(a,3)^2); % Normalized accelerometer 
    Racc_norm1(a,1)=acos(Acc(a,1)/R_norm(a));         % Angle from accelerometer in X
    Racc_norm1(a,2)=acos(Acc(a,2)/R_norm(a));         % Angle from accelerometer in Y
    Racc_norm1(a,3)=acos(Acc(a,3)/R_norm(a));         % Angle from accelerometer in Z

    Gyroscope(a,:)=M.data(i,3:5);                     % Reading gyroscope data
    gyro(a,:)=Gyroscope(a,:)*(1/t);                   % Integrating gyroscope data
    r(a,1)=sum(gyro(:,1));                            % Angle from gyroscope data
    r(a,2)=sum(gyro(:,2));
    r(a,3)=sum(gyro(:,3));
    a=a+1;
end

and compare that with the vectorized form:

Acc = M.data(:,6:8);
R_norm = (Acc(:,1).^2 + Acc(:,2).^2 + Acc(:,3).^2).^(0.5);
Racc_norm1 = acos(Acc(:,1)./repmat(R_norm,1,3));

Gyroscope = M.data(:,3:5);
gyro = Gyroscope.*(1/t);
r = cumsum(gyro);

A couple of things were used there - the colon operator to select 'all' rows (Acc = M.data(:,6:8)), the use of the element-wise operator, which is just a period before the regular operators (.^ .* ./), the use of repmat to get R_norm to be the same size as Acc. repmat is easy to use - the data set you want to copy, how many copies you want in the row dimension, and how many copies you want in the column dimension. Lastly, there's the cumulative sum cumsum to compound sequential sums.

So, in closing, this is probably way more than you were looking for, but former grad student to (probably) grad student, these are tips and tricks I learned along the way with Matlab.

Practice vectorizing code in everything you do. When you're working on small stuff it's easy to see if you've done something wrong. By the time you get to big projects, thinking in vectorized form will be second nature. If you wait for the big project to start trying to vectorize, you're in for a world of pain and sorrow trying to learn on-the-fly or (worse) trying to refactor a project.

Best of luck!

tl;dr - Come for the technical review, stay for the nifty tips!