STM32

一阶，二阶，卡尔曼滤波后的角度效果图

2019-07-21 02:59发布

站内问答 / STM32/STM8
3681 40 1660
一阶滤波

二阶滤波，二阶好像反应好迟钝
卡尔曼滤波

测试过程中，我把mpu6050在+-90°反复转得到上面的角度值，紫 {MOD}是没滤波的，黄的是经过滤波后的
float First_order_filter(float Com_angle,float angle_pt,float angle,float dt);
参数说明：com_angle 滤波后的角度，  angle_pt 角速度（弧度制）， angle 角度(弧度制)， dt 微分时间(秒)
调用       :  com_angle = First_order_filter(com_angle, ang_pt, ang, dtime)

上位机用的是匿名飞控上位机，上位机使用说明请查看9mcu的匿名发的帖子
 
友情提示: 此问题已得到解决，问题已经关闭,关闭后问题禁止继续编辑，回答。
40条回答
x1213378204
2019-07-22 08:58
 回复【7楼】 倒拔萝卜 :
---------------------------------
楼主能不能指导一下上面的卡尔曼滤波，有些地方看不懂 [mw_shl_code=c,true]float Kalman_filter(float Klm_angle,float angle_pt,float angle,float dt)// 卡尔曼滤波 { static float bias; static float P_00,P_01,P_10,P_11; float K_0,K_1; Klm_angle += (angle_pt - bias) * dt; // 先验估计 P_00 += -(P_10 + P_01) * dt + Q_angle *dt; P_01 += -P_11 * dt; P_10 += -P_11 * dt; P_11 += +Q_omega * dt; // 先验估计误差协方差 K_0 = P_00 / (P_00 + R_angle); K_1 = P_10 / (P_00 + R_angle); bias += K_1 * (angle - Klm_angle); Klm_angle += K_0 * (angle - Klm_angle); // 后验估计 P_00 -= K_0 * P_00; P_01 -= K_0 * P_01; P_10 -= K_1 * P_00; P_11 -= K_1 * P_01; // 后验估计误差协方差 return Klm_angle; }[/mw_shl_code] 这个是楼主的例子，我的疑惑是：
1，static float bias和static float P_00,P_01,P_10,P_11，这些是静态局部变量，初始值没给程序怎么启动呢？
2，上面的初值如何确定呢？

一周热门 更多>