注册 登录
查看: 1020|回复: 12

[平衡组] 山外直立中级篇中卡尔曼滤波方案最终输出的角度是angle_dot还是angle??

[复制链接]
发表于 2014-5-11 18:25:47 | 显示全部楼层 |阅读模式
如题,山外直立中级篇中【卡尔曼滤波方案】最终输出的角度是angle_dot还是angle??大家有用过没?
  1. void Kalman_Filter(float angle_m,float gyro_m)          //gyro_m:gyro_measure
  2. {
  3.     angle+=(gyro_m-q_bias) * dt;

  4.     Pdot[0]=Q_angle - P[0][1] - P[1][0];
  5.     Pdot[1]=- P[1][1];
  6.     Pdot[2]=- P[1][1];
  7.     Pdot[3]=Q_gyro;

  8.     P[0][0] += Pdot[0] * dt;
  9.     P[0][1] += Pdot[1] * dt;
  10.     P[1][0] += Pdot[2] * dt;
  11.     P[1][1] += Pdot[3] * dt;


  12.     angle_err = angle_m - angle;



  13.     PCt_0 = C_0 * P[0][0];
  14.     PCt_1 = C_0 * P[1][0];

  15.     E = R_angle + C_0 * PCt_0;

  16.     K_0 = PCt_0 / E;
  17.     K_1 = PCt_1 / E;

  18.     t_0 = PCt_0;
  19.     t_1 = C_0 * P[0][1];

  20.     P[0][0] -= K_0 * t_0;
  21.     P[0][1] -= K_0 * t_1;
  22.     P[1][0] -= K_1 * t_0;
  23.     P[1][1] -= K_1 * t_1;


  24.     angle   += K_0 * angle_err;
  25.     q_bias  += K_1 * angle_err;
  26.     angle_dot = gyro_m-q_bias;
  27. }
回复

使用道具 举报

发表于 2014-5-11 18:48:16 | 显示全部楼层
山外哥的中级篇,虽然有卡斯曼滤波的函数,实际上没用到的,仅仅是清华方案滤波。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-11 18:49:12 | 显示全部楼层
onlyfly 发表于 2014-5-11 18:48
山外哥的中级篇,虽然有卡斯曼滤波的函数,实际上没用到的,仅仅是清华方案滤波。

可以自己调用嘛。我想试试
回复 支持 反对

使用道具 举报

发表于 2014-5-11 18:52:47 | 显示全部楼层
草原上的星星 发表于 2014-5-11 18:49
可以自己调用嘛。我想试试

这个不清楚咯,我没弄过直立,只是粗略看过一下。
回复 支持 反对

使用道具 举报

发表于 2014-5-11 19:55:47 | 显示全部楼层
本帖最后由 gutongxue 于 2014-5-11 20:01 编辑

方法:
把以下代码
QingHua_AngleCalaulate(angle_offset_vertical,Gyro_Now);//清华滤波方案
换成
Kalman_Filter(angle_offset_vertical,Gyro_Now);
就可以了

分析:
函数原型:Kalman_Filter(float angle_m,float gyro_m)
第一个参数是加速度计的角度 第二个是陀螺仪的角速度
最后的输出参数是
float angle, angle_dot;         
第一个是实际的角度,第二个是最终陀螺仪的角速度
然后进行PD参数调节 就可以直立
void Speed_Calculate(float angle,float angle_dot)
{
/***********************************速度计算************************************/
     speed_Start = angle * P_ANGLE  + angle_dot * D_ANGLE ;  //直立时所要的速度

     //P_ANGLE  P_GYRO  宏定义 直立所需要的PD参数

    。。。。。。。。。。。。。。。。。。。。。。        
**************************************************************************************
}

附注:
卡尔曼算法是可以用的,不用清楚理解也能用,按照参数传递下去,看波形图,可以试试,效果很明显的
主要参数调节const float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.005;
主要是调节这个几个参数 有份文档关于卡尔曼参数的调节
我不记得放哪里了,有兴趣的同学可以论坛找一下
回复 支持 反对

使用道具 举报

发表于 2014-5-11 20:04:22 | 显示全部楼层
这个文档可以参考一下

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-11 21:42:08 | 显示全部楼层
危隽洁 发表于 2014-5-11 20:04
这个文档可以参考一下

嗯,非常感谢
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-11 21:42:42 | 显示全部楼层
gutongxue 发表于 2014-5-11 19:55
方法:
把以下代码
QingHua_AngleCalaulate(angle_offset_vertical,Gyro_Now);//清华滤波方案

嗯,强烈支持,非常感谢!!!!
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-11 21:54:16 | 显示全部楼层
gutongxue 发表于 2014-5-11 19:55
方法:
把以下代码
QingHua_AngleCalaulate(angle_offset_vertical,Gyro_Now);//清华滤波方案

这个,还是有一点点疑问哈,就是,为什么清华滤波方案最后输出的只有角度而没有角速度?而卡尔曼滤波最后输出的既有角度又有角速度?是不是使用卡尔曼滤波的效果比清华滤波方案的效果要好?
回复 支持 反对

使用道具 举报

发表于 2014-5-11 21:56:48 | 显示全部楼层
草原上的星星 发表于 2014-5-11 21:54
这个,还是有一点点疑问哈,就是,为什么清华滤波方案最后输出的只有角度而没有角速度?而卡尔曼滤波最后 ...

需要用到的就是角度,不需要角速度,所以清华方案就没有角速度了。
我自己感觉差不了多少,也有的人说卡尔曼效果好一些。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-11 22:06:27 | 显示全部楼层
危隽洁 发表于 2014-5-11 21:56
需要用到的就是角度,不需要角速度,所以清华方案就没有角速度了。
我自己感觉差不了多少,也有的人说卡 ...

角速度需要用的:
  1. void Speed_Calculate(float angle,float angle_dot)
  2. {
  3.     /***********************************速度计算************************************/
  4.     speed_Start = angle * P_ANGLE  + angle_dot * D_ANGLE ;  //直立时所要的速度
  5.     AngleControlOut = speed_Start;
  6.     //P_ANGLE  P_GYRO  宏定义 直立所需要的PD参数


  7. }
不过山外中级篇中用清华滤波方案时,前面就已经算出了角速度,所以清华滤波中没有。
我想问的是,那用卡尔曼滤波后的角速度会不会更好点?因为卡尔曼滤波也是用之前计算的角速度算的,既然它滤波后还输出角速度,那就说明最终输出的那个比之前的要好,是不是这样??
回复 支持 反对

使用道具 举报

发表于 2014-5-15 18:45:02 | 显示全部楼层
两个都用啊,分别用于p d调节
回复 支持 反对

使用道具 举报

发表于 2014-11-16 20:27:54 | 显示全部楼层
.......................
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则

快速回复 返回列表 返回顶部