Hi all,
searching on web i found the code for implementing a Kalman filter. I'd like to implement it in codesys in ST, but im not so skilled in coding so i dont know how to translate that code in ST. Can someone help?
This is the code:

float kalmanFilter(float accAngle, float gyroRate)
{
float y, S;
float K_0, K_1;

KFangle += DT * (gyroRate - _bias);

P_00 += - DT * (P_10 + P_01) + Q_angle * DT;
P_01 += - DT * P_11;
P_10 += - DT * P_11;
P_11 += + Q_gyro * DT;

y = accAngle - KFangle;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;

KFangle += K_0 * y;
_bias += K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;

return KFangle;
}

float Q_angle ;
float Q_gyro ;
float R_angle ;
float _bias ;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float KFangle ;

This is applied for angle prediction based on accelerometer and gyroscope sensor reading. Maybe could be usefull also to other user,
BR
Matteo

hi,Matteo
i find an article,it maybe helpful. see the attachement
Hezk