金牌会员
 
- 积分
- 2504
- 金钱
- 2504
- 注册时间
- 2015-8-17
- 在线时间
- 383 小时
|
楼主 |
发表于 2016-7-21 21:38:07
|
显示全部楼层
[mw_shl_code=c,true]#include "Kalman_filter.h"
#include "math.h"
float Accel_x; //XÖá¼óËù¶èÖμÔY′æ
float Accel_y; //YÖá¼óËù¶èÖμÔY′æ
float Accel_z; //ZÖá¼óËù¶èÖμÔY′æ
float Gyro_x; //XÖáíóÂYòÇêy¾YÔY′æ
float Gyro_y; //YÖáíóÂYòÇêy¾YÔY′æ
float Gyro_z; //ZÖáíóÂYòÇêy¾YÔY′æ
//float Angle_gy; //óé½ÇËù¶è¼ÆËãμÄÇãD±½Ç¶è
float Angle_x_temp; //óé¼óËù¶è¼ÆËãμÄxÇãD±½Ç¶è
float Angle_y_temp; //óé¼óËù¶è¼ÆËãμÄyÇãD±½Ç¶è
float Angle_z_temp;
float Angle_X_Final; //X×îÖÕÇãD±½Ç¶è
float Angle_Y_Final; //Y×îÖÕÇãD±½Ç¶è
float Angle_Z_Final; //Z×îÖÕÇãD±½Ç¶è
//¿¨¶ûÂü2Îêy
char C_0 = 1;
float Q_bias_x, Q_bias_y, Q_bias_z;
float Angle_err_x, Angle_err_y, Angle_err_z;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] = { 0,0,0,0 };
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
double KalmanFilter(const double ResrcData, double ProcessNiose_Q, double MeasureNoise_R)
{
double R = MeasureNoise_R;
double Q = ProcessNiose_Q;
static double x_last;
double x_mid = x_last;
double x_now;
static double p_last;
double p_mid;
double p_now;
double kg;
x_mid = x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)
p_mid = p_last + Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=Ôëéù
kg = p_mid / (p_mid + R); //kg=kalman filter,R=Ôëéù
x_now = x_mid + kg*(ResrcData - x_mid);//1à×îóÅÖμ
p_now = (1 - kg)*p_mid;//×îóÅÖμ¶Ôó|μÄcovariance
p_last = p_now; //¸üDÂcovarianceÖμ
x_last = x_now; //¸üDÂÏμí3×′ì¬Öμ
return x_now;
}
//½Ç¶è¼ÆËã
void Angle_Calcu(void)
{
//·¶Î§Îa2gê±￡¬»»Ëã1ØÏμ￡o16384 LSB/g
//deg = rad*180/3.14
float x=0, y=0, z=0;
Accel_x = aacx; //xÖá¼óËù¶èÖμÔY′æ
Accel_y = aacy; //yÖá¼óËù¶èÖμÔY′æ
Accel_z = aacz; //zÖá¼óËù¶èÖμÔY′æ
Gyro_x = gyrox; //xÖáíóÂYòÇÖμÔY′æ
Gyro_y = gyroy; //yÖáíóÂYòÇÖμÔY′æ
Gyro_z = gyroz; //zÖáíóÂYòÇÖμÔY′æ
//′|àíxÖá¼óËù¶è
if (Accel_x<32764) x = Accel_x / 16384;
else x = 1 - (Accel_x - 49152) / 16384;
//′|àíyÖá¼óËù¶è
if (Accel_y<32764) y = Accel_y / 16384;
else y = 1 - (Accel_y - 49152) / 16384;
//′|àízÖá¼óËù¶è
if (Accel_z<32764) z = Accel_z / 16384;
else z = (Accel_z - 49152) / 16384;
//óüóËù¶è¼ÆËãèy¸öÖáoíˮƽÃæ×ø±êÏμÖ®¼äμļD½Ç
Angle_x_temp = (atan2(z , y)) * 180 / Pi;
Angle_y_temp = (atan2(x , z)) * 180 / Pi;
Angle_z_temp = (atan2(y , x)) * 180 / Pi;
//½Ç¶èμÄÕy¸ooÅ
if (Accel_y<32764) Angle_y_temp = +Angle_y_temp;
if (Accel_y>32764) Angle_y_temp = -Angle_y_temp;
if (Accel_x<32764) Angle_x_temp = +Angle_x_temp;
if (Accel_x>32764) Angle_x_temp = -Angle_x_temp;
if (Accel_z<32764) Angle_z_temp = +Angle_z_temp;
if (Accel_z>32764) Angle_z_temp = -Angle_z_temp;
//½ÇËù¶è
//ÏòÇ°Ô˶ˉ
if (Gyro_x<32768) Gyro_x = -(Gyro_x / 16.4);//·¶Î§Îa1000deg/sê±￡¬»»Ëã1ØÏμ￡o16.4 LSB/(deg/s)
//ÏòoóÔ˶ˉ
if (Gyro_x>32768) Gyro_x = +(65535 - Gyro_x) / 16.4;
//ÏòÇ°Ô˶ˉ
if (Gyro_y<32768) Gyro_y = -(Gyro_y / 16.4);//·¶Î§Îa1000deg/sê±￡¬»»Ëã1ØÏμ￡o16.4 LSB/(deg/s)
//ÏòoóÔ˶ˉ
if (Gyro_y>32768) Gyro_y = +(65535 - Gyro_y) / 16.4;
//ÏòÇ°Ô˶ˉ
if (Gyro_z<32768) Gyro_z = -(Gyro_z / 16.4);//·¶Î§Îa1000deg/sê±￡¬»»Ëã1ØÏμ￡o16.4 LSB/(deg/s)
//ÏòoóÔ˶ˉ
if (Gyro_z>32768) Gyro_z = +(65535 - Gyro_z) / 16.4;
//Angle_gy = Angle_gy + Gyro_y*0.025; //½ÇËù¶è»y·ÖμÃμ½ÇãD±½Ç¶è.Ô½′ó»y·Ö3öà′μĽǶèÔ½′ó
Kalman_Filter_X(Angle_x_temp, Gyro_x); //¿¨¶ûÂüÂË2¨¼ÆËãXÇã½Ç
// yijiehubu_P(Angle_x_temp, Gyro_x);
// Erjielvbo(Angle_x_temp, Gyro_x);
Kalman_Filter_Y(Angle_y_temp, Gyro_y); //¿¨¶ûÂüÂË2¨¼ÆËãYÇã½Ç
Kalman_Filter_Z(Angle_z_temp, Gyro_z); //¿¨¶ûÂüÂË2¨¼ÆËãYÇã½Ç
}
void Kalman_Filter_X(float Accel, float Gyro) //¿¨¶ûÂüoˉêy
{
Angle_X_Final += (Gyro - Q_bias_x) * dt; //ÏèÑé1à¼Æ
Pdot[0] = Q_angle - PP[0][1] - PP[1][0]; // Pk-ÏèÑé1à¼ÆÎó2îD-·½2îμÄÎ￠·Ö
Pdot[1] = -PP[1][1];
Pdot[2] = -PP[1][1];
Pdot[3] = Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-ÏèÑé1à¼ÆÎó2îD-·½2îÎ￠·ÖμÄ»y·Ö
PP[0][1] += Pdot[1] * dt; // =ÏèÑé1à¼ÆÎó2îD-·½2î
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err_x = Accel - Angle_X_Final; //zk-ÏèÑé1à¼Æ
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //oóÑé1à¼ÆÎó2îD-·½2î
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle_X_Final += K_0 * Angle_err_x; //oóÑé1à¼Æ
Q_bias_x += K_1 * Angle_err_x; //oóÑé1à¼Æ
Gyro_x = Gyro - Q_bias_x; //êä3öÖμ(oóÑé1à¼Æ)μÄÎ￠·Ö=½ÇËù¶è
}
void Kalman_Filter_Y(float Accel, float Gyro) //¿¨¶ûÂüoˉêy
{
Angle_Y_Final += (Gyro - Q_bias_y) * dt; //ÏèÑé1à¼Æ
Pdot[0] = Q_angle - PP[0][1] - PP[1][0]; // Pk-ÏèÑé1à¼ÆÎó2îD-·½2îμÄÎ￠·Ö
Pdot[1] = -PP[1][1];
Pdot[2] = -PP[1][1];
Pdot[3] = Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-ÏèÑé1à¼ÆÎó2îD-·½2îÎ￠·ÖμÄ»y·Ö
PP[0][1] += Pdot[1] * dt; // =ÏèÑé1à¼ÆÎó2îD-·½2î
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err_y = Accel - Angle_Y_Final; //zk-ÏèÑé1à¼Æ
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //oóÑé1à¼ÆÎó2îD-·½2î
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle_Y_Final += K_0 * Angle_err_y; //oóÑé1à¼Æ
Q_bias_y += K_1 * Angle_err_y; //oóÑé1à¼Æ
Gyro_y = Gyro - Q_bias_y; //êä3öÖμ(oóÑé1à¼Æ)μÄÎ￠·Ö=½ÇËù¶è
}
void Kalman_Filter_Z(float Accel, float Gyro) //¿¨¶ûÂüoˉêy
{
Angle_Z_Final += (Gyro - Q_bias_z) * dt; //ÏèÑé1à¼Æ
Pdot[0] = Q_angle - PP[0][1] - PP[1][0]; // Pk-ÏèÑé1à¼ÆÎó2îD-·½2îμÄÎ￠·Ö
Pdot[1] = -PP[1][1];
Pdot[2] = -PP[1][1];
Pdot[3] = Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-ÏèÑé1à¼ÆÎó2îD-·½2îÎ￠·ÖμÄ»y·Ö
PP[0][1] += Pdot[1] * dt; // =ÏèÑé1à¼ÆÎó2îD-·½2î
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err_z = Accel - Angle_Z_Final; //zk-ÏèÑé1à¼Æ
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //oóÑé1à¼ÆÎó2îD-·½2î
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle_Z_Final += K_0 * Angle_err_z; //oóÑé1à¼Æ
Q_bias_z += K_1 * Angle_err_z; //oóÑé1à¼Æ
Gyro_z = Gyro - Q_bias_z; //êä3öÖμ(oóÑé1à¼Æ)μÄÎ￠·Ö=½ÇËù¶è
}[/mw_shl_code] |
|
/2 
|手机版|OpenEdv-开源电子网
( 粤ICP备12000418号-1 )
发表于 2016-7-19 11:49:33
发表于 2016-7-20 22:30:52
