MPU6050滤波、姿态融合（一阶互补、卡尔曼）

前几天做了6050原始数据的串口输出和上位机波形的查看。这篇博客我们来看一下对原始数据的处理。

任务：利用STC89C52RC对MPU6050原始数据进行滤波与姿态融合。

首先我摘抄了一段别人在昨晚这个实验的写的最后总结。
1.尽量不要用MPU6050内置的LPF滤波。虽然相比于原始加速度计输出，该LPF可以平滑输出，但是在FFT频谱上的表现相当差劲。
2.广泛使用的窗口平均滑动滤波无论在FFT还是RMSE表现上都有不错的表现，所以一般基础应用（低速运动或四轴初学者）采用窗口平均滤波是比较明智的选择。
3.想要达到更好的滤波效果，FIR或者IIR滤波器是更好的选择。笔者测试2阶30Hz的Butterworth滤波器虽然在平滑性RMSE只比窗口平均滑动差了一点（但是比LPF要好），但是数据实时性性能指标上比前者响应速度提高了近一倍。因此在制作四轴的进阶阶段，可以考虑将窗口平均滑动换成Butterworth滤波器。

开发日记，不想看的可以跳过，仅仅是为了记录自己的过程。
//----------------------------------------------------------------------------------------------------------------------------------------
2017.12.5 晚
刚刚写完6050的零偏矫正和滑动窗口滤波，就已经超过51单片机内部最大RAM允许范围了。具体表现为：
USERS\MPU6050.C(241): error C249: 'DATA': SEGMENT TOO LARGE
定义的数据段大于单片机内部RAM,

查了资料，原来这段程序并没有把stc89c52内部的512字节的RAM全部用完！而是用完了开头的128字节的数据（0X00-0X7F）后面还有128*3的字节量没有被使用。

另外，当我们程序中总变量的字节数超过128字节时，必须对程序中所有变量进行初始化，否则没有被初始化的变量默认值不在是0，而是不确定的随机数。这时我们必须在keil中设置存储器的存储模式。但是当你的变量总字节在128字节以内的话，并且存储模式为small模式时，没有初始化的变量的默认值才是0.

1.Small模式
所有缺省变量参数均装入内部 128字节 RAM，优点是访问速度快，缺点是空间有限，只适用于小程序。
2. Compact 模式
所有缺省变量均位于外部RAM区的一页(256Byte)。和small模式中使用pdata定义变量的效果相同。如：uchar pdata a[100]; 该模式下，总变量不得超过256字节。
3. Large 模式
所有缺省变量可放在多达 64KB 的外部RAM 区，优点是空间大，可存变量多，缺点是速度较慢。在large模式下，和在small模式中使用关键字xdata 定义变量的效果一样。如：char xdata a[100];这样显式的规定了变量的存放区域，以定义为准，不受存储模式的影响。在此模式下编写程序时，定义的变量不要超过单片机规定的内部最大RAM数，因为即使超过了，编译器也不会报错，但是程序运行一定出错。

注意： 为了程序安全，当程序中定义超过128字节的数据时，最好申明为xdata
型，为其在扩展RAM中分配内存。

2017.12.6 下午
一般我们最好配置为small模式，然后必要的情况下，使用xdata对变量进行说明。

/************
注意看编译后的这个，他们的单位时字节B。而1k = 1024B
Program Size: data=16.1 xdata=247 code=3424
/************

由于在6050数据处理函数中多次使用静态局部变量，所以加深一下对他的理解。
对静态局部变量的说明：
(1) 静态局部变量在静态存储区内分配存储单元。在程序整个运行期间都不释放。而自动变量（即动态局部变量）属于动态存储类别，存储在动态存储区空间(而不是静态存储区空间)，函数调用结束后即释放。
(2) 为静态局部变量赋初值是在编译时进行值的，即只赋初值一次，在程序运行时它已有初值。以后每次调用函数时不再重新赋初值而只是保留上次函数调用结束时的值。而为自动变量赋初值，不是在编译时进行的，而是在函数调用时进行，每调用一次函数重新给一次初值，相当于执行一次赋值语句。
(3) 如果在定义局部变量时不赋初值的话，对静态局部变量来说，编译时自动赋初值0(对数值型变量)或空字符(对字符型变量)。而对自动变量来说，如果不赋初值，则它的值是一个不确定的值。这是由于每次函数调用结束后存储单元已释放，下次调用时又重新另分配存储单元，而所分配的单元中的值是不确定的。
(4) 虽然静态局部变量在函数调用结束后仍然存在，但其他函数是不能引用它的，也就是说，在其他函数中它是“不可见”的。

不管怎么修改量程，输出的波形震动范围总是在+-20000波动，但是当我把加速度计的量程设置为+-16G的时候，理论上其LSB应该为2048左右，但是发现输出的波形也是震动范围很大。而且陀螺仪的数据比一开始不处理的时候震动更加明显。

内存覆盖：
以覆盖的（某个函数的局部变量空间在退出该函数是就释放,由别的函数的局部变量覆盖）,可以提高内存利用率。当然静态局部变量除外,其内存使用方式与全局变量相同;

2017.12.7 晚
今天发现我的程序确实存在问题，就是运算时的数据溢出。在进行零偏数据的计算时，需要将200个原始数据相加，但是一开始我的总和定义的还是一个int型的数据，这时200个数据的和可能已经超过了int型数据的取值范围，所以产生了溢出。导致数据的不准确。那么后面计算的数据也是不准确的。所以我晃动传感器曲线都没有什么变化。唉，学单片机都得过数据类型、内存管理这道坎啊。

2017.12.11 下午
如果全是自己开发的话，没有人指导的情况下，每一个小小的进步，都是非常不容易的！！

2017.12.12 晚
目前的问题就是静止状态下陀螺仪输出的数据不为零。还有就是由于单片机运算能力的限制，输出的波形延迟较大。
//---------------------------------------------------------------------------------------------------------------
经过大概一个星期的尝试，终于用51单片机对6050进行了正确的读取与姿态融合。其实现在想想自己这几天所做的所有事情，并不多。程序前两天就写完了，后面的大部分时间都是在对程序进行调整，对波形进行分析。从刚开始凌乱的波形，到后面使用一阶互补滤波和卡尔曼滤波得出一个姿态角的流畅波形。这中间的苦恼和困难只有自己知道。也行对于一个老手来说，我这几天所做的所有事情，他能在一上午或者几个小时内完成，但是这几天所有的思考与疑惑都对我有很大的帮助。慢慢的养成了分析思考问题的方法。遇到问题不要慌张，一点点的去分析。不要放过任何一个细节。其实我们常说，一个经验丰富的老手能很快的解决一个棘手的问题。他们靠的是什么？所有人都知道是经验。但是经验又是什么？说的通俗一点，经验就是他们所有踩过的坑，进行过的所有思考。
想说一个我在做这个实验小小的插曲，有一天的时间，波形出来的都是方波，稍微动一下就满量程，波形很难看。想了很久，检查了不知多少遍代码，逻辑没问题啊，也没有报错，怎么波形就不对呢？突然一瞬间，发现了一点小问题。一个计数变量，定义了一个unsigned char i;来进行零偏数据的计数，每500个数据进行一次平均，刚开始愣是没发现不对。unsigned char 变量的最大计数是256！！！
一个计数变量 ，平时根本就不会去注意他的，但是关键时候就是他搅的局。改了这个错误之后，波形看起来好了很多。

首先在上次博客的基础上，先进行一次滑动平均滤波，滤波窗口大致为6个数据。

/*
**2017.12.5
**对6050的原始数据进行处理，滤波，角度转换。
**滑动平均滤波