Arduino Low Pass Filter Error

Hey guys!.
So basically, I wrote a complementary filter in Arduino, and have uploaded it onto my board. I have a three axis IMU, three axis magnetometer, GPS, barometer, differential pressure and Ultrasonic which run at 50Hz, 50Hz, 1Hz, 9Hz, 10Hz and 20Hz respectively.

Anyways, the whole code is good except I have one problem when I low pass filter the low frequency estimates of a state.
The code/maths behind the low pass filter is fine and works, I have checked it in Matlab etc and is all good. Also the equivelant high pass filter works. And finally the low pass filter WORKS when it is run with input data at 50 Hz. BUT!!!

The issue is when the input data to the Low Pass Filter is lower than 50Hz like 10Hz or 1Hz then it gives the answer as 0 for some reason? But when the input data is 50Hz it works fine.
Now when I say input data is 10Hz I mean the sampling still occurs at 50Hz but you wont see any change in the data over a 0.1 sec time period.. Make sense???

Here is a small exceprt of where you see the low pass filter in operation:

  // High Frequency Estimate of Roll
  float Roll_HE = Roll_HE1 + dt_IMU*((p1 + p)/2.0);
    
  // Low Frequency estimate of Roll
  float Roll_LE = r*V/-9.81; // V is updates at 10Hz, r is at 50Hz
 
  // Complementary Filter of Roll Estimates
  // Low Pass Filter
  float Roll_L = LowPassFilter(Roll_L1,Roll_LE,Roll_LE1,dt_IMU,TimeConst_Roll); 
  // High Pass Filter
  float Roll_H = HighPassFilter(Roll_H1,Roll_HE,Roll_HE1,dt_IMU,TimeConst_Roll);
  // Fusion of High and Low Pass Filters
  float Roll = ToRad(Roll_L + Roll_H);

And the Low Pass Filter:

float LowPassFilter(float yn1,float xn,float xn1,float dt,float TimeConst) {
 
  // Low Pass Filter as a difference equation
  float yn_low = (2.0*TimeConst-dt)/(2.0*TimeConst+dt)*yn1 + dt/(2.0*TimeConst+dt)*xn + dt/(2.0*TimeConst+dt)*xn1;
 
  return yn_low;
}


And the High Pass Filter, which works fine because all the inputs are at 50Hz updates!!!

float HighPassFilter(float yn1,float xn,float xn1,float dt,float TimeConst)
{
 
  // High Pass Filter as a difference equation
  float yn_high = (2.0*TimeConst-dt)/(2.0*TimeConst+dt)*yn1 + 2.0*TimeConst/(2.0*TimeConst+dt)*xn - 2.0*TimeConst/(2.0*TimeConst+dt)*xn1;
 
  return yn_high;
}


The variables in these filters are:
  1. TimeConst = Time Constant, is 1/Cutt Off Freq
  2. dt = Update Rate of Input sensor
  3. xn = Current input, see above code but it may be like the current low frequecy roll or pitch value for example
  4. xn1 = Previous value of xn
  5. yn1 = Previous output of the filter
Hope you can help, otherwise I might cry....
Haha

Thanks in Advance
Joel









You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –

Replies

  • Hello Roy

    r is the yaw rate, and V is the velocity as calculated from the Differential pressure.

    Thanks for the reply, but I have actually figured out the problem and solved it.
    The problem was, when the filter first started up the differential pressure was not giving any outputs, it wasn't giving 0 it just gave NAN (Not a Number). (although if you printed the value and checked it, it would have said 0).
    This was used to calculate V, which was combined with the yaw rate to calculate a roll angle (based on typical aircraft dynamics).
    This filter would run with this NAN input, and therefore the output would also be NAN. The output then is stored in memory for the nest round, (because the filter is a difference equation).
    The next time the filter runs, it calls the previous output of the filter which is NAN, and when it tries to add something to it, it can't because the variable is not a number (even if it is declared as a float, int etc). And hence the new output is also NAN. This goes on forever. So when the differential pressure starts giving data it doesn't matter because the NAN is already in the filter memory.

    I fixed it by putting a if statement on the velocity, so that the velocity must be above 1m/s to use the differential pressure definition of velocity. therefore if the velocity is slower or undefined (NAN) then it ignores the differential pressure velocit (assumes its just 1m/s) and continue. That way no NANs are stored in the memory
  • Joel,

    What are "r" and "V" in this equation below, and how do you get attitude from them?

    >> // Low Frequency estimate of Roll
    >> float Roll_LE = r*V/-9.81; // V is updates at 10Hz, r is at 50Hz

    Here you appear to be averaging the last two values of roll rate before you integrate. Averaging is a low-pass process - not that there's anything wrong with that, but its not technically "high frequency"

    >> // High Frequency Estimate of Roll
    >> float Roll_HE = Roll_HE1 + dt_IMU*((p1 + p)/2.0);


    Also, where did you get the equations for the high and low pass filters? In a complementary filter, the low pass and high pass filter equations should add to 1, and they don't in your implementation.

    The time constant = 1/ cut-off frequency only if the frequency is measured in radians/second. You'd have to divide by 2*pi to get frequency in Hertz. I see you have factors of 2 times your TimeConst, but I'm not sure why. Since you're running at 50 Hz, I presume dt = 1/50 = 0.02 sec. What is the numeric value of TimeConst?

    - Roy
This reply was deleted.

Activity

gotham liked gotham's profile
5 hours ago
DIY Robocars via Twitter
RT @RoboticMasters: Monaco GP Circuit in the Donkey Sim (coming soon). Including buildings, tunnel and all! https://github.com/robotics-masters/sim-donkeycar-f1/tree/f1-tracks @diyr…
yesterday
DIY Robocars via Twitter
RT @breadcentric: Here are the details of #AWSDeepRacer finals: https://blog.deepracing.io/2020/12/01/aws-deepracer-league-finals-2020-round-1-schedule/ #awsreinvent2020 #AWSreInvent https://t.co/ovqsjp8V…
yesterday
DIY Robocars via Twitter
RT @breadcentric: #AWSDeepRacer League #awsreinvent2020 Open race is on Dec 1st - Dec 31st in three categories, 15 DeepRacer Evo (with LIDA…
yesterday
DIY Robocars via Twitter
RT @a1k0n: @SmallpixelCar @diyrobocars It's just something that's easy to track with chroma keying. I ended up using different colors on th…
Monday
DIY Robocars via Twitter
Monday
DIY Robocars via Twitter
RT @TinkerGen_: "The Tinkergen MARK ($199) is my new favorite starter robocar. It’s got everything — computer vision, deep learning, sensor…
Nov 23
DIY Robocars via Twitter
Nov 23
DIY Robocars via Twitter
RT @roboton_io: Join our FREE Sumo Competition 🤖🏆 👉 https://roboton.io/ranking/vsc2020 #sumo #robot #edtech #competition #games4ed https://t.co/WOx…
Nov 16
DIY Drones via Twitter
First impressions of Tinkergen MARK robocar https://ift.tt/36IeZHc
Nov 16
DIY Robocars via Twitter
Our review of the @TinkerGen_ MARK robocar, which is the best on the market right now https://diyrobocars.com/2020/11/15/first-impressions-of-tinkergen-mark-robocar/ https://t.co/ENIlU5SfZ2
Nov 15
DIY Robocars via Twitter
RT @Ingmar_Stapel: I have now explained the OpenBot project in great detail on my blog with 12 articles step by step. I hope you enjoy read…
Nov 15
DIY Robocars via Twitter
RT @DAVGtech: This is a must attend. Click the link, follow link to read the story, sign up. #chaos2020 #digitalconnection #digitalworld ht…
Nov 15
DIY Robocars via Twitter
RT @a1k0n: Got a new chassis for outdoor races (hobbyking Quantum Vandal) but I totally didn't expect that it might cause problems for my g…
Nov 11
DIY Drones via Twitter
First impressions of the Intel OpenBot https://ift.tt/36qkVV4
Nov 10
DIY Robocars via Twitter
Nov 9
More…