MCU ADC sampling algorithm ---- Kalman filter

Regarding Kalman filtering, see the definition on Baidu Encyclopedia

The core idea of ​​the algorithm is to calculate the current optimal value based on the current instrument "measured value" and the "predicted value" and "error" of the previous moment.

When predicting the quantity at the next moment, the most prominent point is that the error is included in the calculation, and it is divided into two types: prediction error and measurement error. It is commonly called noise. Another very big feature is that the error exists independently and is never affected by the measurement data.

Next, let’s understand the meaning of several parameters in a Kalman filter: Probability, Random Variable, Gaussian Distribution, State-space Model, etc.

There are already many articles on the Internet about the meaning and derivation of the Kalman formula. I will not go into details here, but directly look at the implementation of the C code.

/*

    The R value is fixed. The larger the Q value, the more trust the measured value has. The infinite Q value means that only the measured value is used.

             The smaller the Q value, the more trust the model prediction value has. A Q value of 0 means that only the model prediction value is used.

*/

//Parameter 1

float KalmanFilter( float inData )

{

    static float prevData = 0; //Previous data

    static float p = 10, q = 0.001, r = 0.001, kGain = 0; // q controls the error r controls the response speed

    p = p + q;

    kGain = p / ( p + r ); //Calculate Kalman gain

    inData = prevData + ( kGain * ( inData - prevData ) ); //Calculate the estimated value of this filter

    p = ( 1 - kGain ) * p; //Update measurement variance

    prevData = inData;

    return inData; //Return estimated value

}

Now let's test the effect of Kalman filtering. A sawtooth wave is generated by the function generator and sent to the AD port of the microcontroller. After the microcontroller reads the collected AD data, it passes through the Kalman filtering algorithm, and then the sampled data and filtered data are generated through the serial port and displayed on the serial port waveform display software.

void main( void )

{

    while( 1 )

    {

        val1 = ReadVol_CH3(); //Read AD sampling data

        dat = ( float )val1;

        dat = KalmanFilter( dat ); // Kalman filter

        printf("A%drn",val1); //print result

        printf("B%2frn",dat);

    }

}

Now let's look at the results of the filtering

The blue curve is the original sampled data curve, and the orange curve is the curve after Kalman filtering.

Next, change the values ​​of Q and R to test the filtering effect.

The modified parameters are as follows

//Parameter 2

unsigned long kalman_filter( unsigned long ADC_Value )

{

    float LastData;

    float NowData;

    float kalman_adc;

    static float kalman_adc_old = 0;

    static float P1;

    static float Q = 0.0003;

    static float R = 5;

    static float Kg = 0;

    static float P = 1;

    NowData = ADC_Value;

    LastData = kalman_adc_old;

    P = P1 + Q;

    Kg = P / ( P + R );

    kalman_adc = LastData + Kg * ( NowData - kalman_adc_old );

    P1 = ( 1 - Kg ) * P;

    P = P1;

    kalman_adc_old = kalman_adc;

    return ( unsigned long )( kalman_adc );

}

Test waveform

The blue curve is the original sampled data curve, and the orange curve is the curve after Kalman filtering.

By comparing the waveform of the first test with that of the second test, it can be found that the waveform after the second Kalman filter changes greatly. After the parameters are changed, the sawtooth wave is filtered into a nearly straight line.

It can be seen that different R and Q values ​​will have a great impact on the measurement results.

Q: Process noise. As Q increases, the dynamic response becomes faster and the convergence stability becomes worse.

R: measurement noise. As R increases, the dynamic response slows down and the convergence stability improves.

How to select specific parameters can only be adjusted slowly by yourself according to the measurement results in the application. At present, no authoritative information has been found to explain how to select these parameters.