卡尔曼滤波详细代码实现，c语言+matlab

/* * name: 卡尔曼滤波函数 * para: 测量的角度，与该轴测量角速度 * return: 滤波角度 * writer: YXL * function:二阶卡尔曼滤波 * time: 2022/1/22 */ float kalmen_filter(float angle_m ,float gyr_m) { float d_angle,d_gyr;//先验预测量与测量量的差值 //////////////////预测步///////////// ///////期望预测 F1.angle=P1.angle +dt*P1.gyr; F1.gyr=P1.gyr; ///////协方差预测 F1.cov[0]=P1.cov[0]+dt*(P1.cov[1]+P1.cov[2])+Q_angle; F1.cov[1]=P1.cov[1]+dt*P1.cov[3]; F1.cov[2]=P1.cov[2]+dt*P1.cov[3]; F1.cov[3]=P1.cov[3