Kalman Filtering of ADC Sampling Algorithm in MCU

To predict the amount of the next moment, the more prominent point is that the error is included in the calculation, and it is divided into two types: prediction error and measurement error. Commonly known as noise. Another very big feature is that the error exists independently and always does not Affected by measurement data.

Let's first understand the meaning of several parameters in a Kalman filter: probability (Probability), random variable (Random Variable), Gaussian or normal distribution (Gaussian Distribution), and State-space Model, etc.

Regarding the meaning and derivation of Kalman's formula, there are already many articles on the Internet, so I won't go into details here, just look at the implementation of C code.

/*

R值固定，Q值越大，代表越信任测量值，Q值无穷大，代表只用测量值。

         Q值越小，代表越信任模型预测值，Q值为0，代表只用模型预测值。

*/

//Parameter one

float KalmanFilter( float inData )

{

static float prevData = 0;                                 //上一个数据

static float p = 10, q = 0.001, r = 0.001, kGain = 0;      // q 控制误差 r 控制响应速度

p = p + q;

kGain = p / ( p + r );                                      //计算卡尔曼增益

inData = prevData + ( kGain * ( inData - prevData ) );      //计算本次滤波估计值

p = ( 1 - kGain ) * p;                                      //更新测量方差

prevData = inData;

return inData;                                             //返回估计值

}

Now test the effect of Kalman filtering. A sawtooth wave is generated through the function generator and sent to the AD port of the single-chip microcomputer. After the single-chip reads the collected AD data, it passes through the Kalman filtering algorithm, and then combines the sampled data with the filtered one. The data is generated through the serial port and displayed on the serial port waveform display software.

void main( void )

{

while( 1 )

{

    val1 = ReadVol_CH3();            //读取AD采样数据

    dat = ( float )val1;

    dat =    KalmanFilter( dat );    //卡尔曼滤波

    printf("A%drn",val1);          //打印结果

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

}

}

Now look at the result of filtering

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

Let's change the value of Q and R to test the filtering effect.

The modified parameters are as follows

//Parameter two

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.

Compared with the waveform diagram of the first test, it can be found that the waveform after the Kalman filter for the second time has a very large change, and the sawtooth wave is filtered to be close to a straight line after the parameter is changed.

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

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

R: Measurement noise, R increases, dynamic response becomes slower, and convergence stability becomes better

How to choose specific parameters can only be adjusted slowly in the application according to the measurement results.