陀螺仪和加速度的值通过卡尔曼滤波融合.求C程序

给你arduino的卡尔曼滤波融合算法,我只是封装了算法.

另外你这么难的问题应该给点分才厚道啊!

H文件：

/*

* KalmanFilter.h

* Non-original

* Author:x2d

* Copyright (c) 2012 China

*

*/

#ifndef KalmanFilter_h

#define KalmanFilter_h

#include

class KalmanFilter

{

public:

KalmanFilter()

/*

卡尔曼融合计算

angle_m:加速度计测量唤液并通过atan2(ax,ay)方法计算得到的角度（弧度值）

gyro_m:陀螺仪测量的念卖角速度值（弧度值仔链逗）

dt:采样时间(s)

outAngle:卡尔曼融合计算出的角度（弧度值）

outAngleDot:卡尔曼融合计算出的角速度（弧度值）

*/

void getValue(double angle_m,double gyro_m,double dt,double &outAngle,double &outAngleDot)

private:

double C_0,Q_angle,Q_gyro,R_angle

double q_bias,angle_err,PCt_0,PCt_1,E,K_0,K_1,t_0,t_1

double angle,angle_dot

double P[2][2]

double Pdot[4]

}

CPP文件：

/*

* KalmanFilter.cpp

* Non-original

* Author:x2d

* Copyright (c) 2012 China

*

*/

#include "KalmanFilter.h"

KalmanFilter::KalmanFilter()

{

C_0 = 1.0f

Q_angle = 0.001f

Q_gyro = 0.003f

R_angle = 0.5f

q_bias = angle_err = PCt_0 = PCt_1 = E = K_0 = K_1 = t_0 = t_1 = 0.0f

angle = angle_dot = 0.0f

P[0][0] = 1.0f

P[0][1] = 0.0f

P[1][0] = 0.0f

P[1][1] = 1.0f

Pdot[0] = 0.0f

Pdot[1] = 0.0f

Pdot[2] = 0.0f

Pdot[3] = 0.0f

}

void KalmanFilter::getValue(double angle_m,double gyro_m,double dt,double &outAngle,double &outAngleDot)

{

/*

Serial.print("angle_m = ")

Serial.print(angle_m)

Serial.print("")

Serial.print("gyro_m = ")

Serial.print(gyro_m)

Serial.print("")

*/

angle+=(gyro_m-q_bias) * dt

angle_err = angle_m - angle

Pdot[0] = Q_angle - P[0][1] - P[1][0]

Pdot[1] = -P[1][1]

Pdot[2] = -P[1][1]

Pdot[3] = Q_gyro

P[0][0] += Pdot[0] * dt

P[0][1] += Pdot[1] * dt

P[1][0] += Pdot[2] * dt

P[1][1] += Pdot[3] * dt

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 * P[0][1]

P[0][0] -= K_0 * t_0

P[0][1] -= K_0 * t_1

P[1][0] -= K_1 * t_0

P[1][1] -= K_1 * t_1

angle += K_0 * angle_err

q_bias += K_1 * angle_err

angle_dot = gyro_m-q_bias

outAngle = angle

outAngleDot = angle_dot

/*

Serial.print("angle = ")

Serial.print(angle)

Serial.print("")

Serial.print("angle_dot = ")

Serial.print(angle_dot)

Serial.print("")

*/

}

#endif

很简单，角度传感器有模拟和数字之分，模拟型（比如ADXL335,MMA7361）的只需要用AD采集X,Y,X三个方向的电压值值，然后经过公式计算出角度。数字型（比如ADX345L）亏梁按照IIC通信协议，可以直接从传感器内部读取出X,Y,Z三个方向的加速度，然后通过相应的公蔽销式计算出角度。销并运

1.关于6050

陀螺仪和加速度计

的角速度和角度计算。

A.陀螺仪角度计算，很多帖子中都提到了用的是积分，但是我这里还是重新讲下。

angle_n

=

angle_n-1

+

(Gyro-C_Gyro)*R_Gyro

angle_n

当前角度值，它的单位做前是度（°）

angle_n-1上一次计算出的角度值

Gyro陀螺仪敏感轴偏转值，也就是当前敏感轴读数

C_Gyro陀螺仪零点偏移值，这个值的测量方法是：将陀螺仪敏感轴水平放置静止时的读数，我的零点偏移值是水平、垂直、倒置，分简塌别取1024次，作平均值得出的，读数是-177.8865041，但是最后在程序实践中，调整到-99.90。或许还有别的办法，自己看着办吧。

R_Gyro是陀螺仪比例。飞思卡尔的参考中提到这个值是可以计算出来的，下面我会提供下载，大家自己去看看怎么算的，但是在其论坛和调试手册中都提到，这个比例值还是实验法测量出来的比较准确。

B.加速度仪

角度计算。

加速度仪的角度计算有很多方法，论坛里就有2中。但是都用到了三角函数，数学没学好，照抄了也不行。参考了飞思卡尔的计算方法后大概拦胡圆是这样的。

Angle_Z

=

(az-C_Z)*R_Z

angle_z

加速度计敏感轴Z轴产生倾角计算出的角度，单位度（°）

az是加速度仪

Z轴读数

C_Z

Z轴零点偏移量

测量方法和陀螺仪的一样。

R_Z

加速度计Z轴比例

关于陀螺仪的使用感觉还是挺难的，毕竟牵扯的面比较多。如果想学的最好是在论坛里多看看！

---