Problems about Complementary Filter IMU tuning

Question

I'm developing a project consists of an IMU controlled by Arduino through which you can send via a radio module, the data to the PC of the three Euler angles and raw data from the sensors. For filtering I used the code made available by SparkFun: Razor AHRS 9 dof

https://github.com/ptrbrtz/razor-9dof-ahrs/tree/master/Arduino/Razor_AHRS

The code does not provide radio transmissions and is tuned for 50 Hz sampling rate, in fact its parameters are:

// DCM parameters
#define Kp_ROLLPITCH (0.02f)
#define Ki_ROLLPITCH (0.00002f)
#define Kp_YAW (1.2f)
#define Ki_YAW (0.00002f)

in this project data is read every 20ms (50Hz) and records of the sensors are set to the accelerometer odr 50hz and 25 bandwidth. with the gyroscope 50 Hz odr. In my project I used a gyroscope different, namely that I used L3G4200D frequency odr starting at 100Hz, I set then registers with the 100Hz. My global data rate is 33Hz max, beacouse the use of a radio, i read the complete date with a frequency of 33Hz. How can i tune the Ki and Kp of my setup? the Kp is the period, I have to consider the frequency odr that I set to register in the individual sensors or i have to set the global system sample rate limited to 33Hz by the radio transmission?

Answer 1

the Kp is the period

No it's not, Kp is a proportional gain. If your sampling times are very stable, then it becomes possible to modify gains because of the associative properties of multiplication and division. For a PID controller:

$$ \mbox{Controller Output = Proportional Term + Integral Term + Derivative Term} \\ $$ $$ \mbox{Controller Output} = k_P*e_P + k_I*e_I + k_D*e_D \\ $$

where $e_P$, $e_I$, and $e_D$ are the proportional, integral, and derivative error terms, as defined by:

$$ e_{P_k} = \mbox{reference - output} \\ e_{I_k} = e_{I_{k-1}} + e_{P_k}*dT \\ e_{D_k} = e_{P_k}/dT \\ $$

Note that there is no $dT$ term in the proportional error equation! So you do not tune the proportional gain for any given sample time. $k_P$ should always be the same $k_P$. If the starting errors are all set to zero, then the first sample ($k = 1$) for integral and derivative gains should look like the following:

$$ e_{I_1} = e_{I_{0}} + e_P*dT \\ e_{I_1} = 0 + e_P*dT \\ e_{D_1} = e_P/dT \\ $$

Here, because derivative gain never "remembers" any previous states, it should be clear that you can move the sample rate into the gain as follows:

$$ \mbox{Derivative Term} = k_D*eD \\ \mbox{Derivative Term} = k_D*(e_P/dT) \\ \boxed{\mbox{Derivative Term} = (k_D/dT)*e_P} \\ $$

For the integral error, because the initial error is set to zero, try writing out the first few samples to get the pattern:

$$ e_{I_1} = 0 + e_{P_1}*dT \\ e_{I_1} = e_{P_1}*dT \\ $$

$$ e_{I_2} = e_{I_1} + e_{P_2}*dT \\ e_{I_2} = e_{P_1}*dT + e_{P_2}*dT \\ $$

$$ e_{I_3} = e_{I_2} + e_{P_3}*dT \\ e_{I_3} = e_{P_1}*dT + e_{P_2}*dT + e_{P_3}*dT \\ $$

And so again, if the sample rate is constant, you can simplify:

$$ e_{I_N} = \left( \Sigma_{i=1}^N e_{P_i} \right) *dT $$

Which gives:

$$ \mbox{Integral Term} = k_I * e_I \\ \mbox{Integral Term} = k_I * \left( \left( \Sigma_{i=1}^N e_{P_i} \right) *dT \right) \\ \boxed{ \mbox{Integral Term} = \left( k_I *dT \right) * \left( \Sigma_{i=1}^N e_{P_i} \right) }\\ $$

Answer 2

Yes and no.

I just had a quick view at the code which is somehow hard to read what they are actually doing. But in digital Signal processing the Kp value is never normalized to the samplingtime. Instead the Ki is sometimes.... depends on the implementation.

For digital Signal processing the integral term is dividing the IntegralValue by the SampleTime of the system to normalize it for 1second. In this implementation I cannot see such an operation. This means for you yes the I-Term has to be adjusted : Multiply by 0.02 and divide by 0.03/0.01