#include "stm32f10x.h"
#include "Kalman_Filter.h"
//卡尔曼滤波参数与函数
float dt=0.001;//注意：dt的取值为kalman滤波器采样时间
float angle, angle_dot;//角度和角速度
float P[2][2] = {{ 1, 0 },
{ 0, 1 }};
float Pdot[4] ={ 0,0,0,0};
float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度
float R_angle=0.5 ,C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//卡尔曼滤波
float Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy
{
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;//最优角速度
return angle；
}
总结：
三种融合算法都能够输出姿态角（Pitch 和 Roll ），一次四元数法可以输出 P、R、Y 三个倾角，计算量比较大。一阶互补和卡尔曼滤波每次只能输出一个轴的姿态角。
再有这个一阶互补和卡尔曼滤波里的采集后计算的角度和角加速度，上面我们已经讲了，然后再带入到 Kalman_Filter 函数里，那么在C52单片机上实现也不成问题的。
（4）三种滤波比较源代码
参看：MPU6050数据采集及其意义和滤波（一阶互补滤波、二阶互补滤波、卡尔曼滤波）
#include “Wire.h”
#include “I2Cdev.h”
#include “MPU6050.h”
#include “Timer.h”//时间操作系统头文件 本程序用作timeChange时间采集并处理一次数据Timer t;//时间类
float timeChange=20;//滤波法采样时间间隔毫秒
float dt=timeChange*0.001;//注意：dt的取值为滤波器采样时间
// 陀螺仪
float angleAx,gyroGy;//计算后的角度（与x轴夹角）和角速度
MPU6050 accelgyro;//陀螺仪类
int16_t ax, ay, az, gx, gy, gz;//陀螺仪原始数据 3个加速度+3个角速度
//一阶滤波
float K1 =0.05; // 对加速度计取值的权重
//float dt=200.001;//注意：dt的取值为滤波器采样时间
float angle1;//一阶滤波角度输出
//二阶滤波
float K2 =0.2; // 对加速度计取值的权重
float x1,x2,y1;//运算中间变量
//float dt=200.001;//注意：dt的取值为滤波器采样时间
float angle2;//er阶滤波角度输出
//卡尔曼滤波参数与函数
float angle, angle_dot;//角度和角速度
float angle_0, angle_dot_0;//采集来的角度和角速度
//float dt=20*0.001;//注意：dt的取值为kalman滤波器采样时间
//一下为运算中间变量
float P[2][2] = {{ 1, 0 },
{ 0, 1 }};
float Pdot[4] ={ 0,0,0,0};
float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度
float R_angle=0.5 ,C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
void setup() {
Wire.begin();//初始化
Serial.begin(9600);//初始化
accelgyro.initialize();//初始化
int tickEvent1=t.every(timeChange, getangle);//本语句执行以后timeChange毫秒执行回调函数getangleint tickEvent2=t.every(50, printout) ;//本语句执行以后50毫秒执行回调函数printout，串口输出 }
void loop() {
t.update();//时间操作系统运行 }
void printout()
{
Serial.print(angleAx);Serial.print(’,’);
Serial.print(angle1);Serial.print(’,’);
Serial.print(angle2);Serial.print(’,’);
// Serial.print(gx/131.00);Serial.print(’,’);
Serial.println(angle);//Serial.print(’,’);
// Serial.println(Output);
}
void getangle()
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取原始6个数据
angleAx=atan2(ax,az)*180/PI;//计算与x轴夹角
gyroGy=-gy/131.00;//计算角速度
Yijielvbo(angleAx,gyroGy);//一阶滤波
Erjielvbo(angleAx,gyroGy);//二阶滤波
Kalman_Filter(angleAx,gyroGy); //卡尔曼滤波
}
void Yijielvbo(float angle_m, float gyro_m)
{
angle1 = K1 * angle_m+ (1-K1) * (angle1 + gyro_m * dt);
}
void Erjielvbo(float angle_m,float gyro_m)
{
x1=(angle_m-angle2)(1-K2)(1-K2);
y1=y1+x1dt;
x2=y1+2(1-K2)(angle_m-angle2)+gyro_m;
angle2=angle2+ x2dt;
}
void Kalman_Filter(double angle_m,double gyro_m)
{
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;//最优角速度
}
五、欧拉角
参看：姿态角 – 维基百科
参看：姿态角（Euler角）：yaw pitch roll
上面提到的俯仰角 Pitch、滚转角 Roll、偏航角 Yaw、欧拉角等等都是什么呢？
欧拉角又称姿态角（Euler角）并不是指哪个角度，而是是俯仰角、滚转角、偏航角这三个角度的统称。
你可以想象是飞机围绕XYZ三个轴分别转动形成的夹角。
（1）俯仰角θ（pitch）
机体坐标系X轴与水平面的夹角。当X轴的正半轴位于过坐标原点的水平面之上（抬头）时，俯仰角为正，否则为负。
（2）偏航角ψ（yaw）：
机体坐标系xb轴在水平面上投影与地面坐标系xg轴（在水平面上，指向目标为正）之间的夹角，由xg轴逆时针转至机体xb的投影线时，偏航角为正，即机头右偏航为正，反之为负。
（3）滚转角Φ（roll）
机体坐标系zb轴与通过机体xb轴的铅垂面间的夹角，机体向右滚为正，反之为负。
四、姿态融合
参看：MPU6050姿态融合
（下面内容是以STM32开发板来讲的，而我现在使用的C52单片机，有不符合我开发的地方，可做了解）
首先要明确，MPU6050 是一款姿态传感器，使用它就是为了得到待测物体（如四轴、平衡小车） x、y、z 轴的倾角（俯仰角 Pitch、滚转角 Roll、偏航角 Yaw） 。我们通过 I2C 读取到 MPU6050 的六个数据（三轴加速度 AD 值、三轴角速度 AD 值）经过姿态融合后就可以得到 Pitch、Roll、Yaw 角。
本帖主要介绍三种姿态融合算法：四元数法 、一阶互补算法和卡尔曼滤波算法。
（1）四元数法
可通过下面的算法，可以把六个数据转化成四元数（q0、q1、q2、q3），然后四元数转化成欧拉角（P、R、Y 角）。
#include
#include "stm32f10x.h"
//---------------------------------------------------------------------------------------------------
// 变量定义
#define Kp 100.0f // 比例增益支配率收敛到加速度计/磁强计
#define Ki 0.002f // 积分增益支配率的陀螺仪偏见的衔接
#define halfT 0.001f // 采样周期的一半
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计方向
float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差
float Yaw,Pitch,Roll; //偏航角，俯仰角，翻滚角
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
// 测量正常化
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm; //单位化
ay = ay / norm;
az = az / norm;
// 估计方向的重力
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
// 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
// 积分误差比例积分增益
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*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;
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch ,转换为度数
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv
//Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //此处没有价值，注掉
}
说明：
这里 IMUupdate 函数的形参就是我们获取的加速度计和陀螺仪的原始数据，将其带入不就行了吗？说明这个四元数法在C52单片机上也能写一写的。
不过要注意的是，四元数算法输出的是三个量 Pitch、Roll 和 Yaw，运算量很大。而像平衡小车这样的例子只需要一个角（Pitch 或 Roll ）就可以满足工作要求，个人觉得做平衡小车最好不用四元数法。
（2）一阶互补算法
MPU6050 可以输出三轴的加速度和角速度。通过加速度和角速度都可以得到 Pitch 和 Roll 角（加速度不能得到 Yaw 角），就是说有两组 Pitch、Roll 角，到底应该选哪组呢？别急，先分析一下。MPU6050 的加速度计和陀螺仪各有优缺点，三轴的加速度值没有累积误差，且通过算 tan() 可以得到倾角，但是它包含的噪声太多（因为待测物运动时会产生加速度，电机运行时振动会产生加速度等），不能直接使用；陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差。所以，不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角，需要互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值，把它们结合到一起，进行修正。得到 Pitch 角的程序如下：
//一阶互补滤波
float K1 =0.1; // 对加速度计取值的权重
float dt=0.001;//注意：dt的取值为滤波器采样时间
float angle;
angle_ax=atan(ax/az)*57.3; //加速度得到的角度
gy=(float)gyo[1]/7510.0; //陀螺仪得到的角速度
Pitch = yijiehubu(angle_ax,gy);
float yijiehubu(float angle_m, float gyro_m)//采集后计算的角度和角加速度
{
angle = K1 * angle_m + (1-K1) * (angle + gyro_m * dt);
return angle;
}
互补算法只能得到一个倾角，这在平衡车项目中够用了，而在四轴飞行器设计中还需要 Roll 和 Yaw，就需要两个 互补算法，我是这样写的，注意变量不要搞混：
//一阶互补滤波
float K1 =0.1; // 对加速度计取值的权重
float dt=0.001;//注意：dt的取值为滤波器采样时间
float angle_P,angle_R;
float yijiehubu_P(float angle_m, float gyro_m)//采集后计算的角度和角加速度
{
angle_P = K1 * angle_m + (1-K1) * (angle_P + gyro_m * dt);
return angle_P;
}
float yijiehubu_R(float angle_m, float gyro_m)//采集后计算的角度和角加速度
{
angle_R = K1 * angle_m + (1-K1) * (angle_R + gyro_m * dt);
return angle_R;
}
单靠 MPU6050 无法准确得到 Yaw 角，需要和地磁传感器结合使用。
（3）卡尔曼滤波
其实卡尔曼滤波和一阶互补有些相似，输入也是一样的。在此给出具体程序，和一阶互补算法一样，每次卡尔曼滤波只能得到一个方向的角度。
#include
#include "stm32f10x.h"
#include "Kalman_Filter.h"
//卡尔曼滤波参数与函数
float dt=0.001;//注意：dt的取值为kalman滤波器采样时间
float angle, angle_dot;//角度和角速度
float P[2][2] = {{ 1, 0 },
{ 0, 1 }};
float Pdot[4] ={ 0,0,0,0};
float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度
float R_angle=0.5 ,C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//卡尔曼滤波
float Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy
{
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;//最优角速度
return angle；
}
总结：
三种融合算法都能够输出姿态角（Pitch 和 Roll ），一次四元数法可以输出 P、R、Y 三个倾角，计算量比较大。一阶互补和卡尔曼滤波每次只能输出一个轴的姿态角。
再有这个一阶互补和卡尔曼滤波里的采集后计算的角度和角加速度，上面我们已经讲了，然后再带入到 Kalman_Filter 函数里，那么在C52单片机上实现也不成问题的。
（4）三种滤波比较源代码
参看：MPU6050数据采集及其意义和滤波（一阶互补滤波、二阶互补滤波、卡尔曼滤波）
#include “Wire.h”
#include “I2Cdev.h”
#include “MPU6050.h”
#include “Timer.h”//时间操作系统头文件 本程序用作timeChange时间采集并处理一次数据Timer t;//时间类
float timeChange=20;//滤波法采样时间间隔毫秒
float dt=timeChange*0.001;//注意：dt的取值为滤波器采样时间
// 陀螺仪
float angleAx,gyroGy;//计算后的角度（与x轴夹角）和角速度
MPU6050 accelgyro;//陀螺仪类
int16_t ax, ay, az, gx, gy, gz;//陀螺仪原始数据 3个加速度+3个角速度
//一阶滤波
float K1 =0.05; // 对加速度计取值的权重
//float dt=200.001;//注意：dt的取值为滤波器采样时间
float angle1;//一阶滤波角度输出
//二阶滤波
float K2 =0.2; // 对加速度计取值的权重
float x1,x2,y1;//运算中间变量
//float dt=200.001;//注意：dt的取值为滤波器采样时间
float angle2;//er阶滤波角度输出
//卡尔曼滤波参数与函数
float angle, angle_dot;//角度和角速度
float angle_0, angle_dot_0;//采集来的角度和角速度
//float dt=20*0.001;//注意：dt的取值为kalman滤波器采样时间
//一下为运算中间变量
float P[2][2] = {{ 1, 0 },
{ 0, 1 }};
float Pdot[4] ={ 0,0,0,0};
float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度
float R_angle=0.5 ,C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
void setup() {
Wire.begin();//初始化
Serial.begin(9600);//初始化
accelgyro.initialize();//初始化
int tickEvent1=t.every(timeChange, getangle);//本语句执行以后timeChange毫秒执行回调函数getangleint tickEvent2=t.every(50, printout) ;//本语句执行以后50毫秒执行回调函数printout，串口输出 }
void loop() {
t.update();//时间操作系统运行 }
void printout()
{
Serial.print(angleAx);Serial.print(’,’);
Serial.print(angle1);Serial.print(’,’);
Serial.print(angle2);Serial.print(’,’);
// Serial.print(gx/131.00);Serial.print(’,’);
Serial.println(angle);//Serial.print(’,’);
// Serial.println(Output);
}
void getangle()
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取原始6个数据
angleAx=atan2(ax,az)*180/PI;//计算与x轴夹角
gyroGy=-gy/131.00;//计算角速度
Yijielvbo(angleAx,gyroGy);//一阶滤波
Erjielvbo(angleAx,gyroGy);//二阶滤波
Kalman_Filter(angleAx,gyroGy); //卡尔曼滤波
}
void Yijielvbo(float angle_m, float gyro_m)
{
angle1 = K1 * angle_m+ (1-K1) * (angle1 + gyro_m * dt);
}
void Erjielvbo(float angle_m,float gyro_m)
{
x1=(angle_m-angle2)(1-K2)(1-K2);
y1=y1+x1dt;
x2=y1+2(1-K2)(angle_m-angle2)+gyro_m;
angle2=angle2+ x2dt;
}
void Kalman_Filter(double angle_m,double gyro_m)
{
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;//最优角速度
}
五、欧拉角
参看：姿态角 – 维基百科
参看：姿态角（Euler角）：yaw pitch roll
上面提到的俯仰角 Pitch、滚转角 Roll、偏航角 Yaw、欧拉角等等都是什么呢？
欧拉角又称姿态角（Euler角）并不是指哪个角度，而是是俯仰角、滚转角、偏航角这三个角度的统称。
你可以想象是飞机围绕XYZ三个轴分别转动形成的夹角。
（1）俯仰角θ（pitch）
机体坐标系X轴与水平面的夹角。当X轴的正半轴位于过坐标原点的水平面之上（抬头）时，俯仰角为正，否则为负。
（2）偏航角ψ（yaw）：
机体坐标系xb轴在水平面上投影与地面坐标系xg轴（在水平面上，指向目标为正）之间的夹角，由xg轴逆时针转至机体xb的投影线时，偏航角为正，即机头右偏航为正，反之为负。
（3）滚转角Φ（roll）
机体坐标系zb轴与通过机体xb轴的铅垂面间的夹角，机体向右滚为正，反之为负。
举报
举报
