【51单片机快速入门指南】4.3.2: MPU6050:一阶互补滤波、二阶互补滤波和卡尔曼滤波获取欧拉角

目录

  • 源码
    • MPU6050_Filter.c
    • MPU6050_Filter.h
  • 使用方法
  • 测试程序
    • 一阶互补滤波
      • 效果
    • 二阶互补滤波
      • 效果
    • 卡尔曼滤波
      • 效果
  • 总结

普中51-单核-A2
STC89C52
Keil uVision V5.29.0.0
PK51 Prof.Developers Kit Version:9.60.0.0
上位机:Vofa+ 1.3.10


参考资料:
MPU6050数据采集及其意义和滤波(一阶互补滤波、二阶互补滤波、卡尔曼滤波)—— 275891381
关于MPU6050姿态解算的一阶互补滤波方法(从原理到代码实现) —— 可以叫我马同学
姿态融合的一阶互补滤波、二阶互补滤波、卡尔曼滤波核心程序 —— 卖硬件的

源码

       stdint.h见【51单片机快速入门指南】1:基础知识和工程创建
       软件I2C程序见【51单片机快速入门指南】4: 软件 I2C
       串口部分见【51单片机快速入门指南】3.3:USART 串口通信
       MPU6050.c、MPU6050.h见【51单片机快速入门指南】4.3: I2C读取MPU6050陀螺仪的原始数据

MPU6050_Filter.c

#include "MPU6050.h"
#include <math.h>
#include "./MPU6050/MPU6050_Filter.h"#define PI 3.141592653589793float Delta_t = 1;
float GYRO_K = 1;#define First_Order_Filter_Tau 0.075
float First_Order_k = 1;void MPU6050_Filter_Init(float loop_ms)
{Delta_t = loop_ms/1000.;First_Order_k = First_Order_Filter_Tau / (First_Order_Filter_Tau + Delta_t);switch((MPU_Read_Byte(MPU_GYRO_CFG_REG) >> 3) & 3){case 0:GYRO_K = 131;break;case 1:GYRO_K = 65.5;break;case 2:GYRO_K = 32.8;break;case 3:GYRO_K = 16.4;break;}
}float First_Order_Filter_Calc(int16_t acc1, int16_t acc3, int16_t gyro2, float * angle2)
{*angle2 = First_Order_k * (*angle2 + (-gyro2 / GYRO_K) * Delta_t) + (1 - First_Order_k) * (atan2(acc1, acc3) * 180 / PI);return *angle2;
} #define Second_Order_Filter_k 5float Second_Order_Filter_Calc(int16_t acc1, int16_t acc3, int16_t gyro2, Second_Order_Filter* filter)
{float angle_m = atan2(acc1, acc3) * 180 / PI;float gyro_m = -gyro2 / GYRO_K;float x1, x2;x1 = (angle_m - filter->angle) * Second_Order_Filter_k * Second_Order_Filter_k;filter->y = filter->y + x1 * Delta_t;x2 = filter->y + 2 * Second_Order_Filter_k * (angle_m - filter->angle) + gyro_m;filter->angle = filter->angle + x2 * Delta_t;return filter->angle;
}#define Q_angle	0.05	
#define Q_gyro	0.0003	
#define R_angle 0.01	float MPU_Kalman_Filter_Calc(int16_t acc1, int16_t acc3, int16_t gyro2, MPU_Kalman_Filter* filter)
{float newAngle = atan2(acc1, acc3) * 180 / PI;float newRate = -gyro2 / GYRO_K;float E;float K_0, K_1;float Angle_err_x;filter->angle += Delta_t * (newRate - filter->Q_bias_x);filter->P_00 +=  - Delta_t * (filter->P_10 + filter->P_01) + Q_angle * Delta_t;filter->P_01 +=  - Delta_t * filter->P_11;filter->P_10 +=  - Delta_t * filter->P_11;filter->P_11 +=  + Q_gyro * Delta_t;Angle_err_x = newAngle - filter->angle;E = filter->P_00 + R_angle;K_0 = filter->P_00 / E;K_1 = filter->P_10 / E;filter->angle +=  K_0 * Angle_err_x;filter->Q_bias_x  +=  K_1 * Angle_err_x;filter->P_00 -= K_0 * filter->P_00;filter->P_01 -= K_0 * filter->P_01;filter->P_10 -= K_1 * filter->P_00;filter->P_11 -= K_1 * filter->P_01;return filter->angle;
}

MPU6050_Filter.h

#ifndef MPU6050_Filter_H_
#define MPU6050_Filter_H_typedef struct
{float y;float angle;
}Second_Order_Filter;typedef struct
{float P_00, P_01, P_10, P_11;float Q_bias_x;float angle;
}MPU_Kalman_Filter;void MPU6050_Filter_Init(float loop_ms);
float First_Order_Filter_Calc(int16_t acc1, int16_t acc3, int16_t gyro2, float * angle2);
float Second_Order_Filter_Calc(int16_t acc1, int16_t acc3, int16_t gyro2, Second_Order_Filter* filter);
float MPU_Kalman_Filter_Calc(int16_t acc1, int16_t acc3, int16_t gyro2, MPU_Kalman_Filter* filter);#endif

使用方法

       先调用MPU6050_Filter_Init(dt),参数为一次循环的时间,单位为ms
       再使用滤波函数。

测试程序

       生成的程序较大,对于89C52,需要注释掉没用到的函数。

一阶互补滤波

#include <STC89C5xRC.H>
#include "intrins.h"
#include "stdint.h"
#include "USART.h"
#include "./MPU6050/MPU6050.h"
#include "./MPU6050/MPU6050_Filter.h"void Delay1ms()		//@11.0592MHz
{unsigned char i, j;_nop_();i = 2;j = 199;do{while (--j);} while (--i);
}void Delay_ms(int i)
{while(i--)Delay1ms();
}void main(void)
{int16_t aacx,aacy,aacz;		//加速度传感器原始数据int16_t gyrox,gyroy,gyroz;	//陀螺仪原始数据float anglex = 0;float angley = 0;float anglez = 0;USART_Init(USART_MODE_1, Rx_ENABLE, STC_USART_Priority_Lowest, 11059200, 57600, DOUBLE_BAUD_ENABLE, USART_TIMER_1);MPU_Init(); MPU6050_Filter_Init(47);while(1){	MPU_Get_Accelerometer(&aacx, &aacy, &aacz);	//得到加速度传感器数据MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);	//得到陀螺仪数据printf("%f, " , First_Order_Filter(aacy, aacz, gyrox, &anglex));printf("%f, " , First_Order_Filter(aacx, aacz, gyroy, &angley));printf("%f\r\n",First_Order_Filter(aacx, aacy, gyroz, &anglez));}
}

效果

       只看了俯仰和滚转
       First_Order_Filter_Tau 要根据需要调节,我这里取First_Order_Filter_Tau = 0.075
在这里插入图片描述
在这里插入图片描述

二阶互补滤波

#include <STC89C5xRC.H>
#include "intrins.h"
#include "stdint.h"
#include "USART.h"
#include "./MPU6050/MPU6050.h"
#include "./MPU6050/MPU6050_Filter.h"void Delay1ms()		//@11.0592MHz
{unsigned char i, j;_nop_();i = 2;j = 199;do{while (--j);} while (--i);
}void Delay_ms(int i)
{while(i--)Delay1ms();
}Second_Order_Filter anglex = {0, 0}, angley = {0, 0}, anglez = {0, 0};void main(void)
{int16_t aacx,aacy,aacz;		//加速度传感器原始数据int16_t gyrox,gyroy,gyroz;	//陀螺仪原始数据USART_Init(USART_MODE_1, Rx_ENABLE, STC_USART_Priority_Lowest, 11059200, 57600, DOUBLE_BAUD_ENABLE, USART_TIMER_1);MPU_Init(); MPU6050_Filter_Init(56);while(1){	MPU_Get_Accelerometer(&aacx, &aacy, &aacz);	//得到加速度传感器数据MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);	//得到陀螺仪数据printf("%f, " , Second_Order_Filter_Calc(aacy, aacz, gyrox, &anglex));printf("%f, " , Second_Order_Filter_Calc(aacx, aacz, gyroy, &angley));printf("%f\r\n",Second_Order_Filter_Calc(aacx, aacy, gyroz, &anglez));}
}

效果

       只看了俯仰和滚转
       Second_Order_Filter_k根据需要,越大跟随越快,越小越平滑
       (我参考的大佬有取0.8的,有取10的,我这里取5)。
       要根据需要调节
在这里插入图片描述

卡尔曼滤波

#include <STC89C5xRC.H>
#include "intrins.h"
#include "stdint.h"
#include "USART.h"
#include "./MPU6050/MPU6050.h"
#include "./MPU6050/MPU6050_Filter.h"void Delay1ms()		//@11.0592MHz
{unsigned char i, j;_nop_();i = 2;j = 199;do{while (--j);} while (--i);
}void Delay_ms(int i)
{while(i--)Delay1ms();
}MPU_Kalman_Filter anglex = {0};
MPU_Kalman_Filter angley = {0};
MPU_Kalman_Filter anglez = {0};void main(void)
{int16_t aacx,aacy,aacz;		//加速度传感器原始数据int16_t gyrox,gyroy,gyroz;	//陀螺仪原始数据USART_Init(USART_MODE_1, Rx_ENABLE, STC_USART_Priority_Lowest, 11059200, 57600, DOUBLE_BAUD_ENABLE, USART_TIMER_1);MPU_Init(); MPU6050_Filter_Init(76);while(1){	MPU_Get_Accelerometer(&aacx, &aacy, &aacz);	//得到加速度传感器数据MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);	//得到陀螺仪数据printf("%f, " , MPU_Kalman_Filter_Calc(aacy, aacz, gyrox, &anglex));printf("%f, " , MPU_Kalman_Filter_Calc(aacx, aacz, gyroy, &angley));printf("%f\r\n",MPU_Kalman_Filter_Calc(aacx, aacy, gyroz, &anglez));}
}

效果

       只看了俯仰和滚转
       Q参数:过程噪声协方差 Q参数调滤波后的曲线平滑程度,Q越小越平滑;
       R参数:观测噪声协方差 R参数调整滤波后的曲线与实测曲线的相近程度,R越小越接近(收敛越快)
       我参考的大佬有取0.01,0.0003,0.01的,也有取0.001,0.005,0.5的
       我这里取
       Q_angle=0.05
       Q_gyro=0.0003
       R_angle=0.01
       要根据需要调节

在suhetao/stm32f4_mpu9250中有大神对EKF / UKF / CKF / SRCKF的实现,感兴趣的可以看看。
在这里插入图片描述

总结

       由于每种滤波器的参数都会极大地影响该滤波器的性能(一阶滤波、二阶滤波各一个参数,卡尔曼滤波三个参数),因此难以互相比较,我建议根据单片机的资源、性能选择要用的滤波器,调参时配合上位机观察立方体的效果和对应波形

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/400422.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

Mac 运行 psql postgres 报错

psql: could not connect to server: No such file or directoryIs the server running locally and accepting用brew卸载postgresql&#xff1a; brew uninstall postgresqlbrew doctor &#xff08;修正这里的一切&#xff09;brew prune删除所有Postgres文件夹&#xff1a;r…

NGUI基础-三大基础组件之Root组件

NGUI NGUI&#xff08;Next-Gen UI&#xff09;是一款用于Unity游戏引擎的UI插件&#xff0c;它提供了一套功能强大、灵活易用的界面开发工具。在NGUI中&#xff0c;Root&#xff08;根节点&#xff09;是一个重要的概念。 基础组件之Root Root是NGUI中的最高层级节点&#…

【转】android 中如何限制 EditText 最大输入字符数

原文网址&#xff1a;http://blog.csdn.net/fulinwsuafcie/article/details/7437768 方法一&#xff1a; 在 xml 文件中设置文本编辑框属性作字符数限制 如&#xff1a;android:maxLength"10" 即限制最大输入字符个数为10 方法二&#xff1a; 在代码中使用InputFilte…

ibatis 中 $与#的区别

在sql配置中比如in(#rewr#) 与in ($rewr$) 在Ibatis中我们使用SqlMap进行Sql查询时需要引用参数&#xff0c;在参数引用中遇到的符号#和$之间的区分为&#xff0c;#可以进行与编译&#xff0c;进行类型匹配&#xff0c;而$不进行数据类型匹配&#xff0c;例如&#xff1a; sele…

【51单片机快速入门指南】4.3.3: MPU6050使用Mahony AHRS算法实现六轴姿态融合获取四元数、欧拉角

目录源码Mahony_6.cMahony_6.h使用方法测试程序main.c效果STC89C516 32MHz Keil uVision V5.29.0.0 PK51 Prof.Developers Kit Version:9.60.0.0 上位机&#xff1a;Vofa 1.3.10 移植自MPU6050姿态解算——Mahony互补滤波 —— 大写的小写字母 加入了输入数据范围的自动处理…

linux文件系统及bash基础特性

linux文件系统 一、根文件系统 linux被识别的第一个被称为根之间关联的文件系统叫做根文件系统&#xff08;rootfs&#xff09;&#xff0c;其他分区要想被读到&#xff0c;需要挂载到根目录的某个挂载点&#xff08;根的子目录&#xff09;上。根文件系统至关重要&#xff0c;…

WordPress调用自带jquery的方法 ,使 $ 生效

<script>// jQuery.noConflict(); // 由于wordpress 添加了这一行&#xff0c;所以&#xff0c; $ 操作&#xff0c;会报错。(function ($) {function readyFn() {// Set your code here!!}$(document).ready(readyFn); })(jQuery);</script> https://www.kevin…

【51单片机快速入门指南】4.3.4: MPU6050使用Madgwick AHRS算法实现六轴姿态融合获取四元数、欧拉角

目录源码Madgwick_6.cMadgwick_6.h使用方法测试程序main.c效果STC89C516 32MHz Keil uVision V5.29.0.0 PK51 Prof.Developers Kit Version:9.60.0.0 上位机&#xff1a;Vofa 1.3.10 移植自AHRS —— LOXO&#xff0c;算法作者&#xff1a;SOH Madgwick 源码 为了避免所用R…

Hybris商品图片导入与压缩有关的配置.

1. 在电脑上安装 ImageMagick 软件&#xff08;windows平台还需要安装VC&#xff09;&#xff0c;下载路径&#xff1a;http://www.imagemagick.org/script/download.php#windows 在local.properies文件配置安装路径和配置文件路径&#xff1a; Windows版本的&#xff0c;安装…

RTP协议学习大总结从原理到代码

from:http://wenku.baidu.com/view/aaad3d136edb6f1aff001fa5.html 一、流媒体概念 流媒体包含广义和狭义两种内涵&#xff1a;广义上的流媒体指的是使音频和视频形成稳定和连续的传输流和回放流的一系列技术、方法和协议的总称&#xff0c;即流媒体技术&#xff1b;狭义上 的流…

poj 2507Crossed ladders 计算几何

链接&#xff1a;http://poj.org/problem?id2507 题意&#xff1a;哪个直角三角形&#xff0c;一直角边重合&#xff0c; 斜边分别为 X, Y&#xff0c; 两斜边交点高为 C &#xff0c; 求重合的直角边长度~ 思路&#xff1a; 设两个三角形不重合的两条直角边长为 a &#xff0…

响应式样式

/* ##PC##1281px或更高分辨率 */media (min-width: 1281px) {}/* ##笔记本或PC##1025px - 1280px */media (min-width: 1025px) and (max-width: 1280px) {//CSS}/* ##平板电脑/Ipad竖屏##768px - 1024px */media (min-width: 768px) and (max-width: 1024px) {//CSS}/* ##平板…

【机器视觉学习笔记】VS2015 安装 opencv_contrib并测试

目录opencv_contrib的获取主要工具编译 opencv编译 opencv_contribVisual Studio 编译配置新项目的环境添加包含目录添加库目录配置调试环境添加依赖项测试平台&#xff1a;Windows 10 20H2 Visual Studio 2015 opencv_contrib-3.4.12 参考文章&#xff1a; 添加OpenCV_contr…

Windows Server 2012 R2 或 2016 无法安装 .Net 3.5.1

租用阿里云ECS服务器的用户使用 Windows Server 2012 R2 或 Windows Server 2016 64位系统&#xff0c;发现在安装 .net framework 3.5.1 时报错&#xff0c;报错内容如下&#xff1a; 原因分析 找不到安装源文件。 解决办法 可以通过如下 PowerShell 脚本进行安装&#xff1a;…

PHP CURL 多线程 GET/POST 类

PHP CURL 多线程 GET/POST 类 2015-01-01 分类&#xff1a;技术文章 阅读(623) 评论(0)如果有需要更正或更高效的建议&#xff0c;欢迎在OSchina分享~\(≧▽≦)/~ http://www.oschina.net/code/snippet_1475115_44902 <?php /********************************************…

C#中打开设计视图时报未将对象引用设置到对象的实例

通常情况下&#xff0c;若是你将用户控件写好了放入窗体中&#xff0c;若是有不合理的代码&#xff0c;则会弹出错误提示框&#xff0c;不让你放。若是你之前只是随便加了一个用户控件&#xff0c;并且没有什么问题&#xff0c;但后来你又把控件改坏掉了&#xff0c;那么你打开…

C#线程调用带参数的方法,给控件赋值

System.Threading.Thread thread new System.Threading.Thread(() > { //各种业务 //定义一个委托 public delegate void ProcessDelegate(string a); if (this.lbStatus.InvokeRequired) { ProcessDelegate df new ProcessDelegate(AddStatus); this.Invoke(df, new obj…

Concept3D推出交互式3D地图平台

对于活动组织者而言&#xff0c;能够在不必实际旅行的情况下参观活动地点的想法非常具有吸引力&#xff0c;特别是对于日程安排繁忙的人员。Concept3D通过其交互式地图和身临其境的虚拟导览软件实现了这一点。 Concept3D平台的首要位置之一是棕榈泉会议中心&#xff0c;该中心支…

【机器视觉学习笔记】OpenCV C++ 调用笔记本摄像头

目录测试程序实验现象平台&#xff1a;Windows 10 20H2 Visual Studio 2015 opencv_contrib-3.4.12 转自【opencv七】利用opencv调用电脑摄像头 —— yuanCruise 测试程序 #include <opencv2/opencv.hpp> #include <iostream>using namespace cv;int main() {n…

linux文件分割(将大的日志文件分割成小的)

linux下文件分割可以通过split命令来实现&#xff0c;可以指定按行数分割和安大小分割两种模式。Linux下文件合并可以通过cat命令来实现&#xff0c;非常简单。 在Linux下用split进行文件分割&#xff1a; 模式一&#xff1a;指定分割后文件行数 对与txt文本文件&#xff0c;可…