卡尔曼 角度 波型 求指导 谢谢

Eternalve

林JJ 发表于 2014-2-21 14:50
我想问一下，我用的是数字加速度器，放着不动的时候读出来的值会在5的范围内变动正常码？变换好角度之后就可 ...

最大值  最小值是多少呢
5大概占百分之多少呀
如果 5/几千  那应该可以吧

前期数据处理要仔细一点额

林JJ

Eternalve 发表于 2014-2-21 21:54
最大值  最小值是多少呢
5大概占百分之多少呀
如果 5/几千  那应该可以吧

读出来最大最小是-128到127，比如放水平的话它就会在-1到3的范围波动，我还没有进行角度换算

Eternalve

林JJ 发表于 2014-2-22 10:02
读出来最大最小是-128到127，比如放水平的话它就会在-1到3的范围波动，我还没有进行角度换算

应该还行吧  毕竟是数字的   也改不了太多东西是几位精度的呀   可以多读几位哦   8位感觉有点少  12位、14位比较好
可以直接用  愿意的话也可以好好看看手册   初始化的时候设置一下寄存器

林JJ

Eternalve 发表于 2014-2-22 18:25
应该还行吧  毕竟是数字的   也改不了太多东西是几位精度的呀   可以多读几位哦   8位感觉有点少  12位、 ...

谢谢

鸟人

请问楼主 你的卡尔曼得出来的值变化范围多大 我读出来的值才有一点几  是不时正常值？ 还有调的值是不是主要调R ??

Eternalve

鸟人 发表于 2014-2-23 16:55
请问楼主 你的卡尔曼得出来的值变化范围多大 我读出来的值才有一点几  是不时正常值？ 还有调的值是不是主要 ...

我是加速度计arctan算的角度   出来的一点几是弧度（rad）  要自己转化成角度  或者其他度量
前面的那个卡尔曼程序  主要就是调Q R 的相对大小
可以固定Q=1；  调R   
也可以固定R=1；调Q

鸟人

那你的X轴的加速度零点值是陀螺仪水平时还是小车直立时的输出值？ 角速度和Z轴零点值是直立时输出值？？

dingyongbin

vccv

Eternalve

鸟人 发表于 2014-2-24 09:54
那你的X轴的加速度零点值是陀螺仪水平时还是小车直立时的输出值？ 角速度和Z轴零点值是直立时输出值？？

我的“零点”就是小车处于平衡位置不动时加速度计的计算值
不动的时候  陀螺仪处于零点   有绕陀螺仪旋转的运动是    采集到的陀螺仪ad电压值有变化
不知道说到点上了没有

鸟人

我的三个零点值也是平衡时的输出值 我滤出来波如下
黄色是加速度算出来的
红色是角速度  紫色是卡尔曼算出来得角度值
感觉滤出来的波形 粗糙而且还有尖峰
你帮我看看是哪里出问题

程序是这个
    Gyro  = g_ADvalue[0];   
    Acc_x = g_ADvalue[2];
    Acc_z = g_ADvalue[1];
    OutData[1]=Acc_z;
   
   if(Gyro > 4090) Gyro += 1000;
   else
   if(Gyro > 4084) Gyro += 500;
   
   if(Gyro < 55)   Gyro -= 1000;
   else
   if(Gyro < 60)   Gyro -= 500;
      
    Acc_x = Acc_x - 1389.6;
    Acc_z = Acc_z - 2450.0;
    Gyro  = Gyro  - 2860.4;
    Gyro_Data = Gyro;
   
    OutData[0] = Gyro_Data;
    accelerometer_angle =atan2f(Acc_z,Acc_x);
    OutData[2] = accelerometer_angle*1000;

    gyroscope_rate = Gyro*0.0023;         //参考电压3.3v 12位ADC 放大9.1倍 enc-03 0.67mv/deg./sec.
   
                                          //(3300/4096)/(0.67*9.1)*(3.14/180) =  0.0023
   
    NowData = RealData + gyroscope_rate*0.04;                 //1.预估计 X(k|k-1) = A(k,k-1)*X(k-1|k-1) + B(k)*u(k)
    NowData_P = sqrt(Q*Q+RealData_P*RealData_P);              //2.计算预估计协方差矩阵   P(k|k-1) = A(k,k-1)*P(k-1|k-1)*A(k,k-1)'+Q(k)
    Kg = sqrt(NowData_P*NowData_P/(NowData_P*NowData_P+R*R)); //3.计算卡尔曼增益矩阵 K(k) = P(k|k-1)*H(k)' / (H(k)*P(k|k-1)*H(k)' + R(k))
    RealData = NowData + Kg*(accelerometer_angle - NowData);  //4.更新估计 X(k|k) = X(k|k-1)+K(k)*(Z(k)-H(k)*X(k|k-1))
    RealData_P = sqrt((1-Kg)*NowData_P*NowData_P);            //5.计算更新后估计协防差矩阵 P(k|k) =（I-K(k)*H(k)）*P(k|k-1)
   
    QingJiao =  RealData;  
   
   
    OutData[3] = QingJiao*1000;//*180/3.141592653;
   // OutPut_Data();