平衡车的卡尔曼滤波程序是什么意思void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure;angle_dot指的是角速度,即角度求导;{g_fCarAngle+=(gyro_m-q_bias) * dt; //先验估计(积分陀螺仪)// angle_err = ang
来源:学生作业帮助网 编辑:作业帮 时间:2024/11/24 02:37:51
平衡车的卡尔曼滤波程序是什么意思void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure;angle_dot指的是角速度,即角度求导;{g_fCarAngle+=(gyro_m-q_bias) * dt; //先验估计(积分陀螺仪)// angle_err = ang
平衡车的卡尔曼滤波程序是什么意思
void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure;angle_dot指的是角速度,即角度求导;
{
g_fCarAngle+=(gyro_m-q_bias) * dt; //先验估计(积分陀螺仪)
//
angle_err = angle_m - g_fCarAngle; //zk-先验估计(计量角度传感器与陀螺仪的误差)
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E; //Kk
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
g_fCarAngle += K_0 * angle_err; // 后验估计误差协方差; 输出值滤波后的角度
q_bias += K_1 * angle_err; // 后验估计误差协方差
//gyro1=gyro_m-q_bias;
// g_fGyroScopeAngleSpeed = gyro_m-q_bias; //输出值(后验估计)的微分 = 滤波后的角速度
}
这是kalman滤波的程序 我看大家都是这么写的 刚开始看温度那个例子还算是看个半懂 但这个好像和温度那个完全不一样
平衡车的卡尔曼滤波程序是什么意思void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure;angle_dot指的是角速度,即角度求导;{g_fCarAngle+=(gyro_m-q_bias) * dt; //先验估计(积分陀螺仪)// angle_err = ang
看看卡尔曼自己的论文