mpu6050 arduino串口 通讯在ros下的可视化实验

瞎忙好几天，草草做个总结，贴上混乱代码一堆 涉及内容如下

dmp iic  mpu6050的通讯

获取加速度 角速度

二者结合求出角姿势 四元数，发送到计算机。

串口协议编写解析

ros发布位姿里程数据

试验了卡尔曼滤波 ，

加速度积分成位移（漂移问题）

加速度转到频域下积分成位移（高频）参考文章

fft变换卡尔曼滤波等算法

结论：

mpu6050做角姿还不错，求位置很难。

是否是精度问题？噪声问题？低精度信号淹没在大噪声中？

算法问题？没有合适的算法求出纯加速度，进行恰当的滤波？

有待深入学习。

arduino 使用官方库的示例mpu_dmp.ino，使用自定格式串口通讯，部分代码如下；

#ifdef OUTPUT_READABLE_WORLDACCEL
            // display initial world-frame acceleration, adjusted to remove gravity
            // and rotated based on known orientation from quaternion
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);

            byte* pw=(byte*)&(q.w);
            byte* px=(byte*)&(q.x);
            byte* py=(byte*)&(q.y);
            byte* pz=(byte*)&(q.z);

            byte* pwx=(byte*)&(aa.x);
            byte* pwy=(byte*)&(aa.y);
            byte* pwz=(byte*)&(aa.z);
            byte data[26]={'$',0x1a,
              *pw,*(pw+1),*(pw+2),*(pw+3),
              *px,*(px+1),*(px+2),*(px+3),
              *py,*(py+1),*(py+2),*(py+3),
              *pz,*(pz+1),*(pz+2),*(pz+3),
              
              *pwx,*(pwx+1),
              *pwy,*(pwy+1),
              *pwz,*(pwz+1),
              
            '\r', '\n' };
            Serial.write(data, 26);
           /* Serial.print("aworld\t");
            Serial.print(aaWorld.x);
            Serial.print("\t");
            Serial.print(aaWorld.y);
            Serial.print("\t");
            Serial.println(aaWorld.z);*/
        #endif
    

ros  rviz可视化：

冗长的代码：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
//#include "gy85/imu.h"
#include <serial/serial.h>
//ROS已经内置了的串口包

#include <boost/asio.hpp>                  //包含boost库函数
#include "math.h"
#include "helper_3dmath.h"
#include "polyfit.hpp"
#define DOUBLE_PI 6.283185307179586476925286766559
#define DATA_LENGTH 16
// 一维滤波器信息结构体
typedef  struct{
  double a;  //
  double b;   //   Kalamn增益

}  complex;

complex dataX[DATA_LENGTH];
complex dataY[DATA_LENGTH];
complex dataZ[DATA_LENGTH];
complex odataX[DATA_LENGTH];
complex odataY[DATA_LENGTH];
complex odataZ[DATA_LENGTH];
complex r1dataX[DATA_LENGTH];
complex r1dataY[DATA_LENGTH];
complex r1dataZ[DATA_LENGTH];
complex r2dataX[DATA_LENGTH];
complex r2dataY[DATA_LENGTH];
complex r2dataZ[DATA_LENGTH];
double dataXA[DATA_LENGTH];
double dataYA[DATA_LENGTH];
double dataZA[DATA_LENGTH];
double dataXB[DATA_LENGTH];
double dataYB[DATA_LENGTH];
double dataZB[DATA_LENGTH];
void resfft1(){
  unsigned int n=0;
  for(n=0;n<DATA_LENGTH;n++){
    r1dataX[n].a= dataX[n].a;
    r1dataX[n].b= dataX[n].b;
    r1dataY[n].a= dataY[n].a;
    r1dataY[n].b= dataY[n].b;
    r1dataZ[n].a= dataZ[n].a;
    r1dataZ[n].b= dataZ[n].b;
  }
}
void prefft(){

  unsigned int n=0;
  for(n=0;n<DATA_LENGTH;n++){
   dataX[n].a= odataX[n].a;
   dataX[n].b= odataX[n].b;
   dataY[n].a= odataY[n].a;
   dataY[n].b= odataY[n].b;
   dataZ[n].a= odataZ[n].a;
   dataZ[n].b= odataZ[n].b;
  }
}
void c2d(){
  unsigned int n=0;
  for(n=0;n<DATA_LENGTH;n++){
   dataXA[n]= dataX[n].a;
   dataXB[n]= dataX[n].b;
   dataYA[n]= dataY[n].a;
   dataYB[n]= dataY[n].b;
   dataZA[n]= dataZ[n].a;
   dataZB[n]= dataZ[n].b;
  }

}
void d2c(){
  unsigned int n=0;
  for(n=0;n<DATA_LENGTH;n++){
   dataX[n].a=dataXA[n];
   dataX[n].b=dataXB[n];
   dataY[n].a=dataYA[n];
   dataY[n].b=dataYB[n];
   dataZ[n].a=dataZA[n];
   dataZ[n].b=dataZB[n];
  }

}

unsigned char datan=0;

const int N = 1024;
const double PI = 3.14159265358979;

inline void swap (double &a, double &b)
{
    double t;
    t = a;
    a = b;
    b = t;
}

void bitrp (double xreal [], double ximag [], int n)
{
    // 位反转置换 Bit-reversal Permutation
    int i, j, a, b, p;

    for (i = 1, p = 0; i < n; i *= 2)
        {
        p ++;
        }
    for (i = 0; i < n; i ++)
        {
        a = i;
        b = 0;
        for (j = 0; j < p; j ++)
            {
            b = (b << 1) + (a & 1);    // b = b * 2 + a % 2;
            a >>= 1;        // a = a / 2;
            }
        if ( b > i)
            {
            swap (xreal [i], xreal [b]);
            swap (ximag [i], ximag [b]);
            }
        }
}

void FFT(double xreal [], double ximag [], int n)
{
    // 快速傅立叶变换，将复数 x 变换后仍保存在 x 中，xreal, ximag 分别是 x 的实部和虚部
    double wreal [N / 2], wimag [N / 2], treal, timag, ureal, uimag, arg;
    int m, k, j, t, index1, index2;

    bitrp (xreal, ximag, n);

    // 计算 1 的前 n / 2 个 n 次方根的共轭复数 W'j = wreal [j] + i * wimag [j] , j = 0, 1, ... , n / 2 - 1
    arg = - 2 * PI / n;
    treal = cos (arg);
    timag = sin (arg);
    wreal [0] = 1.0;
    wimag [0] = 0.0;
    for (j = 1; j < n / 2; j ++)
        {
        wreal [j] = wreal [j - 1] * treal - wimag [j - 1] * timag;
        wimag [j] = wreal [j - 1] * timag + wimag [j - 1] * treal;
        }

    for (m = 2; m <= n; m *= 2)
        {
        for (k = 0; k < n; k += m)
            {
            for (j = 0; j < m / 2; j ++)
                {
                index1 = k + j;
                index2 = index1 + m / 2;
                t = n * j / m;    // 旋转因子 w 的实部在 wreal [] 中的下标为 t
                treal = wreal [t] * xreal [index2] - wimag [t] * ximag [index2];
                timag = wreal [t] * ximag [index2] + wimag [t] * xreal [index2];
                ureal = xreal [index1];
                uimag = ximag [index1];
                xreal [index1] = ureal + treal;
                ximag [index1] = uimag + timag;
                xreal [index2] = ureal - treal;
                ximag [index2] = uimag - timag;
                }
            }
        }
}

void  IFFT (double xreal [], double ximag [], int n)
{
    // 快速傅立叶逆变换
    double wreal [N / 2], wimag [N / 2], treal, timag, ureal, uimag, arg;
    int m, k, j, t, index1, index2;

    bitrp (xreal, ximag, n);

    // 计算 1 的前 n / 2 个 n 次方根 Wj = wreal [j] + i * wimag [j] , j = 0, 1, ... , n / 2 - 1
    arg = 2 * PI / n;
    treal = cos (arg);
    timag = sin (arg);
    wreal [0] = 1.0;
    wimag [0] = 0.0;
    for (j = 1; j < n / 2; j ++)
        {
        wreal [j] = wreal [j - 1] * treal - wimag [j - 1] * timag;
        wimag [j] = wreal [j - 1] * timag + wimag [j - 1] * treal;
        }

    for (m = 2; m <= n; m *= 2)
        {
        for (k = 0; k < n; k += m)
            {
            for (j = 0; j < m / 2; j ++)
                {
                index1 = k + j;
                index2 = index1 + m / 2;
                t = n * j / m;    // 旋转因子 w 的实部在 wreal [] 中的下标为 t
                treal = wreal [t] * xreal [index2] - wimag [t] * ximag [index2];
                timag = wreal [t] * ximag [index2] + wimag [t] * xreal [index2];
                ureal = xreal [index1];
                uimag = ximag [index1];
                xreal [index1] = ureal + treal;
                ximag [index1] = uimag + timag;
                xreal [index2] = ureal - treal;
                ximag [index2] = uimag - timag;
                }
            }
        }

    for (j=0; j < n; j ++)
        {
        xreal [j] /= n;
        ximag [j] /= n;
        }
}
void swap(complex *v1, complex *v2)
{
    complex tmp = *v1;
    *v1 = *v2;
    *v2 = tmp;
}

void fftshift(complex *data, int count)
{
    int k = 0;
    int c = (int) floor((float)count/2);
    // For odd and for even numbers of element use different algorithm
    if (count % 2 == 0)
    {
        for (k = 0; k < c; k++)
            swap(&data[k], &data[k+c]);
    }
    else
    {
        complex tmp = data[0];
        for (k = 0; k < c; k++)
        {
            data[k] = data[c + k + 1];
            data[c + k + 1] = data[k + 1];
        }
        data[c] = tmp;
    }
}

void ifftshift(complex *data, int count)
{
    int k = 0;
    int c = (int) floor((float)count/2);
    if (count % 2 == 0)
    {
        for (k = 0; k < c; k++)
            swap(&data[k], &data[k+c]);
    }
    else
    {
        complex tmp = data[count - 1];
        for (k = c-1; k >= 0; k--)
        {
            data[c + k + 1] = data[k];
            data[k] = data[c + k];
        }
        data[c] = tmp;
    }
}
void detrend(double array[], int n)
{
     double x, y, a, b;
     double sy = 0.0,
            sxy = 0.0,
            sxx = 0.0;
     int i;

     for (i=0, x=(-(double)n/2.0+0.5); i<n; i++, x+=1.0)
     {
          y = array[i];
          sy += y;
          sxy += x * y;
          sxx += x * x;
     }
     b = sxy / sxx;
     a = sy / n;

     for (i=0, x=(-(double)n/2.0+0.5); i<n; i++, x+=1.0)
          array[i] -= (a+b*x);
}
void iomega_expCount(complex  dataX[],complex iomega_array[],double iomega_exp){
  for(int n=0;n<DATA_LENGTH;n++){
    double a,b,c,d;
    a=dataX[n].a;
    b=dataX[n].b;
    c=iomega_array[n].a;
    d=iomega_array[n].b;
    if(iomega_array[n].a!=0||iomega_array[n].b!=0){

      //A(j) = A(j) * (iomega_array(j) ^ iomega_exp);
      //^
      double r =sqrt(pow(c, 2) + pow(d, 2));
      double sita =atan(d/c);
      double R = pow(r, iomega_exp)*cos(iomega_exp*sita );
      double I = pow(r, iomega_exp)*sin(iomega_exp*sita );
      //return Complex(R, I);
      c=R;
      d=I;
      //*
      //(ac－bd)+(bc+ad)i
      dataX[n].a=(a*c-b*d);
      dataX[n].b=(b*c+a*d);
    }else{
      dataX[n].a=0;
      dataX[n].b=0;
    }
  }
}
//void  iomega(datain, dt, datain_type, dataout_type)
void  iomega( double dt, int datain_type,int dataout_type){
  double N =(double) DATA_LENGTH; //2^nextpow2(max(size(datain)));
  double df = 1/(N*dt);
  double Nyq = 1/(2*dt);
  complex iomega_array [DATA_LENGTH];
  for(int c=0;c<(int)N;c++){
    //
    iomega_array[c].a=0;
    iomega_array[c].b=2.0*DOUBLE_PI*(-Nyq+(double)c*df);
  }
  //iomega_array = 1i*2*pi*(-Nyq : df : Nyq-df);
  int iomega_exp = dataout_type - datain_type;
  //iomega_exp = dataout_type - datain_type;
  prefft();
  c2d();
  FFT(dataXA,dataXB,DATA_LENGTH);
  FFT(dataYA,dataYB,DATA_LENGTH);
  FFT(dataZA,dataZB,DATA_LENGTH);

  d2c();
  fftshift(dataX,DATA_LENGTH);
  fftshift(dataY,DATA_LENGTH);
  fftshift(dataZ,DATA_LENGTH);



  iomega_expCount(dataX,iomega_array,iomega_exp);
  iomega_expCount(dataY,iomega_array,iomega_exp);
  iomega_expCount(dataZ,iomega_array,iomega_exp);
  //
  ifftshift(dataX,DATA_LENGTH);
  ifftshift(dataY,DATA_LENGTH);
  ifftshift(dataZ,DATA_LENGTH);
  c2d();
  IFFT(dataXA,dataXB,DATA_LENGTH);
  IFFT(dataYA,dataYB,DATA_LENGTH);
  IFFT(dataZA,dataZB,DATA_LENGTH);
  d2c();

}
std::vector<double> dxa(DATA_LENGTH);
std::vector<double> dya(DATA_LENGTH);
std::vector<double> dza(DATA_LENGTH);
std::vector<double> dxb(DATA_LENGTH);
std::vector<double> dyb(DATA_LENGTH);
std::vector<double> dzb(DATA_LENGTH);
void res12v(){

  unsigned int n=0;
  for(n=0;n<DATA_LENGTH;n++){
    dxa[n]=r1dataX[n].a;
    dya[n]=r1dataY[n].a;
    dza[n]=r1dataZ[n].a;
    dxb[n]=r1dataX[n].b;
    dyb[n]=r1dataY[n].b;
    dzb[n]=r1dataZ[n].b;
  }
}
std::vector<double> rdx(DATA_LENGTH),rdy(DATA_LENGTH),rdz(DATA_LENGTH),rdx2(DATA_LENGTH),rdy2(DATA_LENGTH),rdz2(DATA_LENGTH);
std::vector <double> vel_avrg_result(3);
std::vector <double> result(3);
double velintX[DATA_LENGTH];
double velintY[DATA_LENGTH];
double velintZ[DATA_LENGTH];
void count(){
  //% 频域积分
      double ts=0.02;
      //velint =  iomega(acc, ts, 3, 2);
      iomega( ts, 3, 2);
      resfft1();
      //velint = detrend(velint);
      //ROS_INFO("----------------");
      for(int i=0;i<DATA_LENGTH;i++){
        velintX[i]=r1dataX[i].b;
        velintY[i]=r1dataY[i].b;
        velintZ[i]=r1dataZ[i].b;
        //ROS_INFO("%d :%lf %lf %lf",i,velintX[i],velintY[i],velintZ[i]);
      }

      //detrend(velintX,1);
      //detrend(velintY,1);
      //detrend(velintZ,1);
      for(int i=0;i<DATA_LENGTH;i++){
        vel_avrg_result[0]+=velintX[i];
        vel_avrg_result[1]+=velintY[i];
        vel_avrg_result[2]+=velintZ[i];
        //ROS_INFO("%d :%lf %lf %lf",i,vel_avrg_result[0],vel_avrg_result[1],vel_avrg_result[2]);
      }
//vel_avrg_result[0]/=DATA_LENGTH;
//vel_avrg_result[1]/=DATA_LENGTH;
//vel_avrg_result[2]/=DATA_LENGTH;
//ROS_INFO("%d :%lf %lf %lf",100,vel_avrg_result[0],vel_avrg_result[1],vel_avrg_result[2]);
      //disint =  iomega( ts, 3, 1);
      iomega( ts, 3, 1);
      resfft1();
      res12v();
      ROS_INFO("contok");
      //% 去除位移中的二次项
      //p = polyfit(t, disint, 2);
      rdx=polyfit(dxa, dxb, 2);
      rdy=polyfit(dya, dyb, 2);
      rdz=polyfit(dza, dzb, 2);
      //disint = disint - polyval(p, t);
res12v();
      rdx2=polyval(rdx,dxa);
      rdy2=polyval(rdy,dya);
      rdz2=polyval(rdz,dza);
      for(int i=0;i<DATA_LENGTH;i++){
        rdx[i]-=rdx2[i];
        rdy[i]-=rdy2[i];
        rdz[i]-=rdz2[i];
      }

      result[0]=rdx[15];
      result[1]=rdy[15];
      result[2]=rdz[15];
    //return result;*/
}
// 一维滤波器信息结构体
typedef  struct{
  double filterValue;  //k-1时刻的滤波值，即是k-1时刻的值
  double kalmanGain;   //   Kalamn增益
  double A;   // x(n)=A*x(n-1)+u(n),u(n)~N(0,Q)
  double H;   // z(n)=H*x(n)+w(n),w(n)~N(0,R)
  double Q;   //预测过程噪声偏差的方差
  double R;   //测量噪声偏差，(系统搭建好以后，通过测量统计实验获得)
  double P;   //估计误差协方差
}  KalmanInfo;
/**
* @brief Init_KalmanInfo   初始化滤波器的初始值
* @param info  滤波器指针
* @param Q 预测噪声方差 由系统外部测定给定
* @param R 测量噪声方差 由系统外部测定给定
*/
void Init_KalmanInfo(KalmanInfo* info, double Q, double R)
{
  info->A = 1;  //标量卡尔曼
  info->H = 1;  //
  info->P = 10;  //后验状态估计值误差的方差的初始值（不要为0问题不大）
  info->Q = Q;    //预测（过程）噪声方差 影响收敛速率，可以根据实际需求给出
  info->R = R;    //测量（观测）噪声方差 可以通过实验手段获得
  info->filterValue = 0;// 测量的初始值
}
double KalmanFilter(KalmanInfo* kalmanInfo, double lastMeasurement)
{
  //预测下一时刻的值
  double predictValue = kalmanInfo->A* kalmanInfo->filterValue;   //x的先验估计由上一个时间点的后验估计值和输入信息给出，此处需要根据基站高度做一个修改

  //求协方差
  kalmanInfo->P = kalmanInfo->A*kalmanInfo->A*kalmanInfo->P + kalmanInfo->Q;  //计算先验均方差 p(n|n-1)=A^2*p(n-1|n-1)+q
  double preValue = kalmanInfo->filterValue;  //记录上次实际坐标的值

  //计算kalman增益
  kalmanInfo->kalmanGain = kalmanInfo->P*kalmanInfo->H / (kalmanInfo->P*kalmanInfo->H*kalmanInfo->H + kalmanInfo->R);  //Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)
  //修正结果，即计算滤波值
  kalmanInfo->filterValue = predictValue + (lastMeasurement - predictValue)*kalmanInfo->kalmanGain;  //利用残余的信息改善对x(t)的估计，给出后验估计，这个值也就是输出  X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))
  //更新后验估计
  kalmanInfo->P = (1 - kalmanInfo->kalmanGain*kalmanInfo->H)*kalmanInfo->P;//计算后验均方差  P[n|n]=(1-K[n]*H)*P[n|n-1]

  return  kalmanInfo->filterValue;
}
KalmanInfo klmx;
KalmanInfo klmy;
KalmanInfo klmz;

using namespace boost::asio;           //定义一个命名空间，用于后面的读写操作
using namespace std;
serial::Serial ser; //声明串口对象
unsigned char buf[1];                      //定义字符串长度
unsigned char data[512];//
unsigned int dn=0;
unsigned int i=0,j=0;

  double pitch=0, roll=0, yaw=0;
  double ax = 0.0;
  double ay = 0.0;
  double az = 0.0;
  double tsx = 0.0;
  double tsy = 0.0;
  double tsz = 0.0;
  double tvx = 0.0;
  double tvy = 0.0;
  double tvz = 0.0;

  Vectordouble oawV;
  Quaternion worldQ; //[w,x,y,z]
  Vectordouble aV;//[x,y,z]
  Vectordouble gV;//[x,y,z]
  Vectordouble gVo;//[x,y,z]
  Vectordouble realV;


  void countGravity(Quaternion worldQ){
    gV.x=2.0*(worldQ.x*worldQ.z-worldQ.w*worldQ.y);
    gV.y=2.0*(worldQ.w*worldQ.x+worldQ.y*worldQ.z);
    gV.z=worldQ.w*worldQ.w-worldQ.x*worldQ.x-worldQ.y*worldQ.y+worldQ.z*worldQ.z;
  }

  void countRealAccel(short int awx, short int awy, short int awz){

    realV.x=((double)awx)-(gV.x)*4096.0;
    realV.y=((double)awy)-(gV.y)*4096.0;
    realV.z=((double)awz)-(gV.z)*4096.0;
    gVo.x = gV.x;
    gVo.y = gV.y;
    gVo.z = gV.z;
    oawV.x=(double)awx;
    oawV.y=(double)awy;
    oawV.z=(double)awz;
  };

  double gravity[3]={0.0,0.0,0.0};
  double linear_acceleration[3]={0.0,0.0,0.0};

  void onSensorChanged(double realx,double realy,double realz){
    // 在本例中，alpha 由 t / (t + dT)计算得来，
    // 其中 t 是低通滤波器的时间常数，dT 是事件报送频率
    float alpha = 0.8;
    // 用低通滤波器分离出重力加速度
    gravity[0]  = alpha * gravity[0] + (1 - alpha) * realx;
    gravity[1]  = alpha * gravity[1] + (1 - alpha) * realy;
    gravity[2]  = alpha * gravity[2] + (1 - alpha) * realz;
    // 用高通滤波器剔除重力干扰
    linear_acceleration[0] = realx - gravity[0];
    linear_acceleration[1] = realy - gravity[1];
    linear_acceleration[2] = realz - gravity[2];
    aV.x=linear_acceleration[0];
    aV.y=linear_acceleration[1];
    aV.z=linear_acceleration[2];

  }


  double acc2v(short int aw){
    return double (((double) aw)/4096.0)*9.8;
  }
  double acc2v(double aw){
    return double ((aw)/4096.0)*9.8;
  }



  ros::Time last_time;
  unsigned int startN=100;
  tf::Quaternion lastQ;

  void pulishMSG( short int awx, short int awy, short int awz,float yy,float pp,float rr,float qw,float qx,float qy,float qz,tf::TransformBroadcaster odom_broadcaster,ros::Publisher odom_pub){
    ros::Time current_time= ros::Time::now();
    double dt=(current_time.toSec()-last_time.toSec());


    last_time=current_time;

    worldQ.w=double(qw);
    worldQ.x=double(qx);
    worldQ.y=double(qy);
    worldQ.z=double(qz);
    worldQ.normalize();

    countGravity(worldQ);
    countRealAccel(awx, awy,awz);


    //ROS_INFO("awx=%d awy=%d awz=%d ",awx,awy,awz);
    //ROS_INFO("realx=%lf realy=%lf realz=%lf ",realV.x,realV.y,realV.z);
    onSensorChanged( realV.x, realV.y, realV.z);
    //onSensorChanged( double(awx), double(awy), double(awz));
    //ROS_INFO("linearx=%lf lineary=%lf linearz=%lf ",linear_acceleration[0],linear_acceleration[1],linear_acceleration[2]);

    if(startN==0){
      aV.rotate(&worldQ);
      ax=acc2v(aV.x);
      ay=acc2v(aV.y);
      az=acc2v(aV.z);


      odataX[datan].a=((datan==0)?0.0:(odataX[datan-1].a+dt));
      odataY[datan].a=odataX[datan].a;
      odataZ[datan].a=odataX[datan].a;
      odataX[datan].b=ax;
      odataY[datan].b=ay;
      odataZ[datan].b=az;
      datan+=1;
      if(datan==DATA_LENGTH){
        //
        ROS_INFO("----------------");
        for(datan=0;datan<DATA_LENGTH;datan++){
          ROS_INFO("dax=%lf day=%lf daz=%lf ",odataX[datan],odataY[datan],odataZ[datan]);

        }
        datan=0;
        count();
        //std::vector <double> posi=count();
        ROS_INFO("posix=%lf posiy=%lf posiz=%lf ",result[0],result[1],result[2]);
        //tsx+= result[0];
        //tsy+= result[1];
        //tsz+= result[2];
        //tsx+= vel_avrg_result[0]*DATA_LENGTH*dt;
        //tsy+= vel_avrg_result[1]*DATA_LENGTH*dt;
        //tsz+= vel_avrg_result[2]*DATA_LENGTH*dt;
        //ROS_INFO("vx=%lf vy=%lf vz=%lf ",vel_avrg_result[0],vel_avrg_result[1],vel_avrg_result[2]);

      }
      //
      ax=KalmanFilter(&klmx, ax);
      ay=KalmanFilter(&klmy, ay);
      az=KalmanFilter(&klmz, az);

      tvx +=ax*dt;
      tvy +=ay*dt;
      tvz +=az*dt;

    }else{
      startN--;
    }


    //tsx= tvx;
    //tsy= tvy;
    //tsz= tvz;
    tsx+= tvx*dt;
    tsy+= tvy*dt;
    tsz+= tvz*dt;

    if(tsx*tsx+tsy*tsy+tsz*tsz>3*3){
      tsx= 0.0;
      tsy= 0.0;
      tsz= 0.0;
    }
    tvx*=0.9;
    tvy*=0.9;
    tvz*=0.9;
    /*ax*=0.9;
    ay*=0.9;
    az*=0.9;
    */


    //tf::Quaternion Q=tf::createQuaternionFromRPY(0.0,0.0,0.0);//	( qx,      qy,      qz,      qw     ) ;
    //Q.setEulerZYX(0,0,3.1415926/180);
    //Q.x=double(nqx);
    //Q.y=double(nqy);
    //Q.z=double(nqz);
    //Q.w=double(nqw);
    geometry_msgs::Quaternion odom_quat= tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0);

    odom_quat.x=worldQ.x;//Q.getX();
    odom_quat.y=worldQ.y;//Q.getY();
    odom_quat.z=worldQ.z;//Q.getZ();
    odom_quat.w=worldQ.w;//Q.getW();


    //first, we'll publish the transform over tf
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    odom_trans.transform.translation.x = tsx*10;
    odom_trans.transform.translation.y = tsy*10;
    odom_trans.transform.translation.z = tsz*10;
    odom_trans.transform.rotation = odom_quat;

    //send the transform
    odom_broadcaster.sendTransform(odom_trans);

    //next, we'll publish the odometry message over ROS
    nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

    //set the position
    odom.pose.pose.position.x = tsx*10;
    odom.pose.pose.position.y = tsy*10;
    odom.pose.pose.position.z = tsz*10;//odom_trans.transform.translation.z;
    odom.pose.pose.orientation = odom_quat;

    //set the velocity
    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = tvx;
    odom.twist.twist.linear.y = tvy;
    odom.twist.twist.linear.z = tvz;
    odom.twist.twist.angular.z = 0.0;

    //publish the message
    odom_pub.publish(odom);
  }
int main(int argc, char** argv){
  ros::init(argc, argv, "mpu6050_teapotPacket");

  ros::NodeHandle n;
  ros::Publisher odom_pub;
  tf::TransformBroadcaster odom_broadcaster;
  odom_pub = n.advertise<nav_msgs::Odometry>("odom", 1);

  Init_KalmanInfo(&klmx,0.5,0.1);
  Init_KalmanInfo(&klmy,0.5,0.1);
  Init_KalmanInfo(&klmz,0.5,0.1);


  io_service iosev;
  serial_port sp(iosev, "/dev/ttyUSB0");         //定义传输的串口
  sp.set_option(serial_port::baud_rate(115200));
  sp.set_option(serial_port::flow_control());
  sp.set_option(serial_port::parity());
  sp.set_option(serial_port::stop_bits());
  sp.set_option(serial_port::character_size(8));
  iosev.run();

  ros::Rate r(200.0);
  last_time=ros::Time::now();
  lastQ=tf::createQuaternionFromRPY(0.0,0.0,0.0);
  while(n.ok()){



    while(1){

      try
      {
        read (sp,buffer(buf));
        data[dn]=buf[0];
        dn++;

        if(dn>=64){break;}
        //ROS_INFO("%d", buf[0]);//打印接受到的字符串
      }catch (boost::system::system_error e){
          ROS_ERROR_STREAM("read err ");
          break;
      }
    }
    //ROS_INFO("read");
    float qw,qx,qy,qz;
    float yy,pp,rr;
    short int  awx,awy,awz;
    unsigned char *pwx,*pwy,*pwz;

    unsigned char * pw,*px,*py,*pz;
    unsigned char * pyy,*ppp,*prr;
    unsigned char flag=0;
    //xieyi
    //{'$',l,x,x,y,y,z,z,w,w,0,0,'\r','\n'}
        for(i=0;i<(dn-1);){
          if((data[i])=='$'){
            unsigned char len=data[i+1];
            if((i+len)>dn)break;//data length is not ok
            if((data[i+len-2]!='\r')||(data[i+len-1]!='\n')){
              i++;
              continue;//package error
            };
            //
            //ROS_INFO("d0=%d",'$');
            if(len==26){

                pw=(unsigned char*)&qw;
                px=(unsigned char*)&qx;
                py=(unsigned char*)&qy;
                pz=(unsigned char*)&qz;
                *pw=data[i+2];
                *(pw+1)=data[i+3];
                *(pw+2)=data[i+4];
                *(pw+3)=data[i+5];
                *px=data[i+6];
                *(px+1)=data[i+7];
                *(px+2)=data[i+8];
                *(px+3)=data[i+9];
                *py=data[i+10];
                *(py+1)=data[i+11];
                *(py+2)=data[i+12];
                *(py+3)=data[i+13];
                *pz=data[i+14];
                *(pz+1)=data[i+15];
                *(pz+2)=data[i+16];
                *(pz+3)=data[i+17];

                pwx=(unsigned char*)&awx;
                pwy=(unsigned char*)&awy;
                pwz=(unsigned char*)&awz;
                *pwx=data[i+18];
                *(pwx+1)=data[i+19];
                *pwy=data[i+20];
                *(pwy+1)=data[i+21];
                *pwz=data[i+22];
                *(pwz+1)=data[i+23];
                flag=1;
                i+=25;
             }
            if(len==16){


                pyy=(unsigned char*)&yy;
                ppp=(unsigned char*)&pp;
                prr=(unsigned char*)&rr;

                *pyy=data[i+2];
                *(pyy+1)=data[i+3];
                *(pyy+2)=data[i+4];
                *(pyy+3)=data[i+5];
                *ppp=data[i+6];
                *(ppp+1)=data[i+7];
                *(ppp+2)=data[i+8];
                *(ppp+3)=data[i+9];
                *prr=data[i+10];
                *(prr+1)=data[i+11];
                *(prr+2)=data[i+12];
                *(prr+3)=data[i+13];

                flag=1;
                i+=15;

            }
            if(len==20){

                pw=(unsigned char*)&qw;
                px=(unsigned char*)&qx;
                py=(unsigned char*)&qy;
                pz=(unsigned char*)&qz;
                *pw=data[i+2];
                *(pw+1)=data[i+3];
                *(pw+2)=data[i+4];
                *(pw+3)=data[i+5];
                *px=data[i+6];
                *(px+1)=data[i+7];
                *(px+2)=data[i+8];
                *(px+3)=data[i+9];
                *py=data[i+10];
                *(py+1)=data[i+11];
                *(py+2)=data[i+12];
                *(py+3)=data[i+13];
                *pz=data[i+14];
                *(pz+1)=data[i+15];
                *(pz+2)=data[i+16];
                *(pz+3)=data[i+17];
                flag=1;
                i+=19;
             }
            }else{
              //wait
              break;
            }



          i+=1;
        }
        //move to 0
        //if(i>0){
        for(j=0;i<dn;i++,j++){
        data[j]=data[i];
        }
        dn=j;
        if(flag){
          //ROS_INFO("w=%f x=%f y=%f z=%f",qw,qx,qy,qz);
          pulishMSG(awx,awy,awz,yy,pp,rr, qw, qx, qy, qz,odom_broadcaster,odom_pub);
        }
    ros::spinOnce();

    r.sleep();
  }
}