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);
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!!!// 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;
}
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:{
// 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;
}
- 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
Haha
Thanks in Advance
Joel
Replies
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
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