weixin_44849612 2019-10-28 11:15 采纳率: 100%
浏览 1679
已采纳

关于mpu9250原始数据的滤波

因为加速度计和磁力计存在高频误差,需要低通滤波器来输出准确的数据,陀螺仪存在低频误差,需要进行高通滤波,但是我看了CSDN上的帖子,也没帖子明确这些滤波的截止频率,所以想来这请教一下各位。

图片说明
这个是我在帖子上看到的关于mpu9250解算姿态的流程,我最后的姿态融合用的是AHRS算法,进行了磁力计的补偿,但是不知道这种情况下还需要进行第二步(磁力计坐标系到加速度坐标系)吗?需要的画,相关的资料哪位大哥有啊,有的话能提供一下吗?救救老弟吧,真的谢谢了。
下面是我用的AHRS算法代码:

// 加速度计、地磁计、陀螺仪数据融合,更新四元数
/*
   [gx,gy,gz]为陀螺仪的测量值
   [ax,at,az]为加速度的测量值
   [mx,my,mz]为地磁计的测量值 
*/
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) 
{  
            float norm;               
            float hx, hy, hz, bx, bz;
            float vx, vy, vz, wx, wy, wz; 
            float ex, ey, ez;  

            // 定义一些辅助变量用于转换矩阵
            float q0q0 = q0*q0;  
            float q0q1 = q0*q1;  
            float q0q2 = q0*q2;  
            float q0q3 = q0*q3;  
            float q1q1 = q1*q1;  
            float q1q2 = q1*q2;  
            float q1q3 = q1*q3;  
            float q2q2 = q2*q2;  
            float q2q3 = q2*q3;  
            float q3q3 = q3*q3;  

            // 归一化加速度计和地磁计的度数 
            norm = sqrt(ax*ax + ay*ay + az*az);   
            ax = ax / norm;  
            ay = ay / norm;  
            az = az / norm;  
            norm = sqrt(mx*mx + my*my + mz*mz);   
            mx = mx / norm;  
            my = my / norm;  
            mz = mz / norm;  

            //将b系中的地磁计分量[mx,my,mz]转换到n系,得到[hx,hy,hz]  
            hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);  
            hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);  
            hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);        

            //得到n系中的地磁向量的真实值[bx,bz,by],其中by=0   
            bx = sqrt((hx*hx) + (hy*hy));  
            bz = hz;     

            //n系中的地磁向量[bx,by,bz]转换到b系中,得到[wx,wy,wz]
            wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);  
            wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);  
            wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);                        

            //n系中重力加速度[0,0,1]转换到b系中得到三个分量[vx,vy,vz]        
            vx = 2*(q1q3 - q0q2);  
            vy = 2*(q0q1 + q2q3);  
            vz = q0q0 - q1q1 - q2q2 + q3q3;    

            //计算[wx,wy,wz] X [mx,my,mz],[ax,at,az] X [vx,vy,vz],得到两个误差后求和
            ex = (ay*vz - az*vy) + (my*wz - mz*wy);  
            ey = (az*vx - ax*vz) + (mz*wx - mx*wz);  
            ez = (ax*vy - ay*vx) + (mx*wy - my*wx);  

            //PI控制器中的积分部分
            exInt = exInt + ex*Ki* (1.0f / sampleFreq);  
            eyInt = eyInt + ey*Ki* (1.0f / sampleFreq);  
            ezInt = ezInt + ez*Ki* (1.0f / sampleFreq);  

            //误差经过PI控制器后输出,然后补偿到角速度的三个分量,Kp、Ki是需要调节的参数
            gx = gx + Kp*ex + exInt;  
            gy = gy + Kp*ey + eyInt;  
            gz = gz + Kp*ez + ezInt;               

            //一阶龙格库塔法更新四元数  
            q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;  
            q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;  
            q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;  
            q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;    

            // 归一化四元数
            norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);  
            q0 = q0 / norm;  
            q1 = q1 / norm;  
            q2 = q2 / norm;  
            q3 = q3 / norm;  
}  

  • 写回答

2条回答 默认 最新

  • weixin_43716859 2019-11-01 12:20
    关注

    一般磁力计和加速度计的截止频率设置为30HZ就行,或者你用8深度的窗口滑动滤波也可以,更方便,这个是窗口平均滑动滤波的代码https://blog.csdn.net/enjoy_learn/article/details/80509866

    本回答被题主选为最佳回答 , 对您是否有帮助呢?
    评论
查看更多回答(1条)

报告相同问题?

悬赏问题

  • ¥20 求一个html代码,有偿
  • ¥100 关于使用MATLAB中copularnd函数的问题
  • ¥20 在虚拟机的pycharm上
  • ¥15 jupyterthemes 设置完毕后没有效果
  • ¥15 matlab图像高斯低通滤波
  • ¥15 针对曲面部件的制孔路径规划,大家有什么思路吗
  • ¥15 钢筋实图交点识别,机器视觉代码
  • ¥15 如何在Linux系统中,但是在window系统上idea里面可以正常运行?(相关搜索:jar包)
  • ¥50 400g qsfp 光模块iphy方案
  • ¥15 两块ADC0804用proteus仿真时,出现异常