求角速度的积分方法或者代码，我用mpu6050来获得模块的角速度，但是不知道怎样积分

woi314ow

如果可以的话最好有代码，虽然我买了直接用重力加速度来进行计算姿态的模块但是我想做一个更加精确的陀螺仪，所以我想知道对代码进行角速度积分的方法。

woi314ow

回复【6楼】阿拓:
---------------------------------

void Kalman_Filter(float Gyro,float Accel) //角速度，加速度
{
//验估计
Angle+=(Gyro - Q_bias) * dt;           //上一刻角度加上（角速度-误差）*时间
//协方差矩阵的预测
Pdot[0]=Q_angle - P[0][1] - P[1][0]; /
Pdot[1]= - P[1][1];
Pdot[2]= - P[1][1];/
Pdot[3]=Q_gyro;

PP[0][0] += dot[0] * dt;   
PP[0][1] += dot[1] * dt; 
PP[1][0] += dot[2] * dt;
PP[1][1] += dot[3] * dt;  
//  通过卡尔曼增益进行修整
Angle_err = Accel - Angle;

PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;
K_1 = PCt_1 / E;

t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
//更新协方差阵
PP[0][0] -= K_0 * t_0;
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;

Angle += K_0 * Angle_err;
Q_bias += K_1 * Angle_err;
Gyro_x   = Gyro - Q_bias;
}





首先是卡尔曼滤波的5个方程

X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)//先验估计
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)//协方差矩阵的预测
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (3)//计算卡尔曼增益
X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1)) ……… (4)通过卡尔曼增益进行修正
P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)//跟新协方差阵

卡尔曼滤波的5个方程
1. 验估计
2. 协方差矩阵预测
3. 计算卡尔曼增益
4. 通过卡尔曼增益进行修整
5. 根新协方差阵

5个式子比较抽象，现在直接用实例来说
—，对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间，因为 ，角度微分等于时间的微分乘以角速度。

但是陀螺仪有个静态漂移（而且还是变化的），静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。
由此我们得到了当前角度的预测值 Angle
Angle=Angle+(Gyro - Q_bias) * dt;  
其中等号左边Angle为此时的角度，等号右边Angle为上一时刻的角度，Gyro 为陀螺仪测的角速度的值，dt是两次滤波之间的时间间隔。
float  dt=0.005;                   这是程序中的定义
同时 Q_bias也是一个变化的量。


但是就预测来说认为现在的漂移跟上一时刻是相同的即
Q_bias=Q_bias
将两个式子写成矩阵的形式
 
得到上式，这个式子对应于卡尔曼滤波的第一个式子
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)//先验估计
X(k|k-1)为2维列向量 ，A为2维方阵 ，X(k-1|k-1)为2维列向量 ，B 为2维列向量 ，U(k) 为Gyro


二，这里是卡尔曼滤波的第二个式子

接着是预测方差阵的预测值，这里首先要给出两个值，一个是漂移的噪声，一个是角度值的噪声，（所谓噪声就是数据的方差值）
P(k|k-1)=A P(k-1|k-1) A’+Q 
这里的Q为向量  的协方差矩阵，即 
因为漂移噪声还有角度噪声是相互独立的，则 =0； =0
又由性质可知cov（x，x）=D（x）即方差，所以得到的矩阵如下
 ，这里的两个方差值是开始就给出的常数
程序中的定义如下float  Q_angle=0.001;  
                 float  Q_gyro=0.003;

接着是这一部分A P(k-1|k-1) A’,其中的（P（k-1）|P(k-1)）为上一时刻的预测方差阵
卡尔曼滤波的目标就是要让这个预测方差阵最小。
其中P(k-1|k-1)设为 ,第一式已知A为 
则计算A P(k-1|k-1) A’+Q（就是个矩阵乘法和加法，算算吧）结果如下
 
 很小为了计算简便忽略不计。
于是得到
 
a,b,c,d分别和矩阵的P[0][0],P[0][1],P[1][0],P[1][1]
计算过程转化为如下程序，代换即可

    Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; 
Pdot[1]= - PP[1][1];
Pdot[2]= - PP[1][1];/
Pdot[3]=Q_gyro;

PP[0][0] += Pdot[0] * dt;   
PP[0][1] += Pdot[1] * dt; 
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;  

三，这里是卡尔曼滤波的第三个式子
 Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (3)//计算卡尔曼增益


即计算卡尔曼增益，这是个二维向量设为 ，这里的      =  为由此kg=
P(K|K-1)+R，这里又有一个常数R，程序中的定义如下
float  R_angle=0.5;
这个指的是角度测量噪声值，则式子的分母=P[0][0]+R_angle
即程序中的

PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
分子  
于是求出 
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
四，用误差还有卡尔曼增益来修正
X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1)) ……… (4)通过卡尔曼增益进行修正
这个矩阵带进去就行了Z（k)=Accel.....注意这个是加速度计算出来的角度
Angle_err = Accel - Angle;
对应程序如下
Angle += K_0 * Angle_err;
Q_bias += K_1 * Angle_err;
同时为了PID控制还有下次的使用把角速度算出来了
Gyro_x   = Gyro - Q_bias;

五，最后一步对矩阵P进行更新，因为下一次滤波时要用到
PP[0][0] -= K_0 * t_0;
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)//跟预测方差阵
这个很简单，矩阵带进去算就行了
六，总结
卡尔曼滤波一共只需要给很少的初始值量，float  Q_angle=0.001;  
                                      float  Q_gyro=0.003;
还有float  R_angle=0.5;
以及系统的初始量angle还有Q_bias
还有预测误差矩阵P，程序里给的是0（数组）
理论上由于卡尔曼滤波是迭代的算法，当时间充分长以后。滤波估值将与初始值的选取无关。
但是实际上并不是如此，比如测量方差值一直在变化。

其实卡尔曼的程序相当的简单，
只要你理解了他的那5条公式。

zuozhongkai

开一个定时器，2m周期，每2ms采集一次角速度，然后乘以2ms的时间，就可以认为是这2ms转过的角度了，这个就是在单片机中的积分思路。如果你还需要更精确一点的话就把定时器频率调大一点，不过这个积分结果随着时间的增加累积误差会越来越大的。

woi314ow

回复【2楼】zuozhongkai:
---------------------------------
这个我还是有点清楚的，不过我想问问，你写过这个代码没

zuozhongkai

写过，用与四轴的

woi314ow

回复【4楼】zuozhongkai:
---------------------------------
能分享一下代码不!

阿拓

回复【5楼】woi314ow:
---------------------------------
楼主能否分享一下6050采集的代码，我也在弄这个传感器，折腾一个星期了数据还没有读出来。

slay0you

晕，难道不是用泰勒级数，展开成级数吗？

woi314ow

回复【7楼】slay0you:
---------------------------------
啥！能用那个东西？

阿拓

回复【9楼】woi314ow:
---------------------------------
谢谢啦

楼下是老王

学习了

FANXS

woi314ow 发表于 2015-3-26 10:51
回复【6楼】阿拓:
---------------------------------

void Kalman_Filter(float Gyro,float&nb ...

内容好像有点缺失，有些地方没有显示出来

yingyuzuoye

woi314ow 发表于 2015-3-26 10:51
回复【6楼】阿拓:
---------------------------------

void Kalman_Filter(float Gyro,float&nb ...

跑题。全网都是算法介绍，很难翻到t是怎么得到的。读了小马哥的四轴源码，虽然没看懂，但应该不是一个大循环加毫秒延时来积分的。上群里问，一会任务间隔一会时间片，都懵了。