平衡车的卡尔曼滤波程序是什么意思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/06/27 04:33:41
![平衡车的卡尔曼滤波程序是什么意思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](/uploads/image/z/10433876-68-6.jpg?t=%E5%B9%B3%E8%A1%A1%E8%BD%A6%E7%9A%84%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2%E7%A8%8B%E5%BA%8F%E6%98%AF%E4%BB%80%E4%B9%88%E6%84%8F%E6%80%9Dvoid+Kalman_Filter%28float+angle_m%2Cfloat+gyro_m%29+%2F%2Fgyro_m%3Agyro_measure%EF%BC%9Bangle_dot%E6%8C%87%E7%9A%84%E6%98%AF%E8%A7%92%E9%80%9F%E5%BA%A6%2C%E5%8D%B3%E8%A7%92%E5%BA%A6%E6%B1%82%E5%AF%BC%EF%BC%9B%7Bg_fCarAngle%2B%3D%28gyro_m-q_bias%29+%2A+dt%3B+%2F%2F%E5%85%88%E9%AA%8C%E4%BC%B0%E8%AE%A1%28%E7%A7%AF%E5%88%86%E9%99%80%E8%9E%BA%E4%BB%AA%29%2F%2F+angle_err+%3D+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 = 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
看看卡尔曼自己的论文