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:
- TimeConst = Time Constant, is 1/Cutt Off Freq
- dt = Update Rate of Input sensor
- xn = Current input, see above code but it may be like the current low frequecy roll or pitch value for example
- xn1 = Previous value of xn
- yn1 = Previous output of the filter
Hope you can help, otherwise I might cry....
Haha
Thanks in Advance
Joel