现在:在静止状态下经kalman融合后的得到的角度与通过加速度直接转换的角度都是正常的,在转动传感器后可以发现经加速度直接转换的角度值正常,可是经过kalman滤波融合后的角度先变成很大在变到正常值。
例如,传感器初始角度为10
加速度转换的角度为10
kalman融合后的角度为10
转动传感器到50
加速度转换角度直接变为50
kalman融合后的角度变化:10 >> 20 >>40 >>60 >>70 >>60 >>50
这个是什么情况啊。。。
代码如下
/*************卡尔曼滤波***************/
void Kalman_Filter1(float Accel1,float Gyro1)
{
static const float Q_angle=0.001;
static const float Q_gyro=0.003;
static const float R_angle=0.5;
static const float dt=0.01; //dt为kalman滤波器采样时间单位;秒;
static const char C_0 = 1;
static float Q_bias, Angle_err;
static float PCt_0, PCt_1, E;
static float K_0, K_1, t_0, t_1;
static float Pdot[4] ={0,0,0,0};
static float PP[2][2] = { { 1, 0 },{ 0, 1 } };
Angle_x+=(Gyro1 - Q_bias) * dt; //根据角速度估计这段时间内的角度变化
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先检验估计误差协方差微分
Pdot[1]= -PP[1][1];
Pdot[2]= -PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先检验估计误差协方差积分
PP[0][1] += Pdot[1] * dt;
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel1 - Angle_x; //zk-检验估计值
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; //最后检验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle_x += K_0 * Angle_err; //传感器姿态角度
Q_bias += K_1 * Angle_err; //后验估计
Gyro_y = Gyro1 - Q_bias; //输出值的微分=角速度
}
MPU6050_ReadAcc( &g_global.sensor.u16Acc[0], &g_global.sensor.u16Acc[1], &g_global.sensor.u16Acc[2]);
MPU6050_ReadGyro( &g_global.sensor.u16Gyro[0], &g_global.sensor.u16Gyro[1], &g_global.sensor.u16Gyro[2]);
Angle_ax = g_global.sensor.u16Acc[0]*1.55*180/(16384*3.14);
Gyro_y = -g_global.sensor.u16Gyro[1]/16.4;
Kalman_Filter1(Angle_ax,Gyro_y);
友情提示: 此问题已得到解决,问题已经关闭,关闭后问题禁止继续编辑,回答。
一周热门 更多>