上一篇文章有讲到卡尔曼滤波了，现在需要将其添加到我们之前的C52测试程序中。
STM32 相关工程，下载：STM32F10x 卡尔曼滤波
一、再看一下卡尔曼滤波程序
#include<math.h>
#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；
}
或者：
#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=20*0.001;//注意：dt的取值为滤波器采样时间
float angle1;//一阶滤波角度输出
//二阶滤波
float K2 =0.2; // 对加速度计取值的权重
float x1,x2,y1;//运算中间变量
//float dt=20*0.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毫秒执行回调函数getangle
int 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+x1*dt;
x2=y1+2*(1-K2)*(angle_m-angle2)+gyro_m;
angle2=angle2+ x2*dt;
}
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;//最优角速度
}
二、解析
卡尔曼滤波函数有两个参数，它的实参定义为 angleAx，gyroGy;//计算后的角度（与x轴夹角）和角速度
那么如何计算angleAx，gyroGy？
（1）angleAx 计算
angleAx=atan2(ax,az)*180/PI; //计算与x轴夹角 PI 为 3.14
ax、az是什么？
MPU6050初始化设置范围为2g时，灵敏度 16384 LSB/g
ax = ACCEL_XOUT_H / 16384
az = ACCEL_ZOUT_H / 16384
因此可以这样写：
angleAx=atan2(（ACCEL_XOUT_H / 16384）,（ACCEL_ZOUT_H / 16384）)*180/3.14;
（2）gyroGy 计算
gyroGy=-gy/131.00; //计算角速度
注意，131.00 是当陀螺仪量程为± 250 °/s时的灵敏度 131 LSB/°/s。
MPU6050初始化设置范围为± 2000 °/s时，灵敏度为 16.4 LSB/°/s。
gy是什么？
gy = GYRO_YOUT_H
因此可以这样写：
gyroGy=-GYRO_YOUT_H/16.4;
（3）卡尔曼滤波函数
而这样的一个 Kalman_Filter(angleAx, gyroGy); 卡尔曼滤波，每次卡只能得到一个方向的角度。
如此说来，我们再获取其他两个角度即可。
具体上面这三个与姿态角（Euler角）yaw pitch roll 对应关系，我还没搞清楚...
但大体上卡尔曼滤波和C52测试程序就是这样子结合的。只待进一步验证了...
三、编写程序
参看：MPU6050 卡尔曼滤波
//****************************************
// Update to MPU6050 by shinetop
// MCU: STC89C52
// 2012.3.1
// 功能: 显示加速度计和陀螺仪的10位原始数据
//****************************************
// 使用单片机STC89C52
// 晶振：11.0592M
// 显示：串口
// 编译环境 Keil uVision2
//****************************************
#include <REG52.H>
#include <math.h> //Keil library
#include <stdio.h> //Keil library
#include <INTRINS.H>
typedef unsigned char uchar;
typedef unsigned short ushort;
typedef unsigned int uint;
//****************************************
// 定义51单片机端口
//****************************************
sbit SCL=P1^5; //IIC时钟引脚定义
sbit SDA=P1^4; //IIC数据引脚定义
//****************************************
// 定义MPU6050内部地址
//****************************************
#define SMPLRT_DIV 0x19 //陀螺仪采样率，典型值：0x07(125Hz)
#define CONFIG 0x1A //低通滤波频率，典型值：0x06(5Hz)
#define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
#define ACCEL_CONFIG 0x1C //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x42
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRO_ZOUT_L 0x48
#define PWR_MGMT_1 0x6B //电源管理，典型值：0x00(正常启用)
#define WHO_AM_I 0x75 //IIC地址寄存器(默认数值0x68，只读)
#define SlaveAddress 0xD0 //IIC写入时的地址字节数据，+1为读取
//**************************************************************************************************
//定义类型及变量
//**************************************************************************************************
uchar dis[6]; //显示数字(-511至512)的字符数组
int dis_data; //变量
//******角度参数************
float Gyro_y; //Y轴陀螺仪数据暂存
float Angle_gy; //由角速度计算的倾斜角度
float Accel_x;
//X轴加速度值暂存
float Angle_ax; //由加速度计算的倾斜角度
float Angle; //小车最终倾斜角度
uchar value; //角度正负极性标记
//**************************************************************************************************
//函数声明
//**************************************************************************************************
void Delay5us();
void delay(unsigned int k); //延时
void lcd_printf(uchar *s,int temp_data);
//********************************MPU6050操作函数***************************************************
void InitMPU6050(); //初始化MPU6050
void I2C_Start();
void I2C_Stop();
void I2C_SendACK(bit ack);
bit I2C_RecvACK();
void I2C_SendByte(uchar dat);
uchar I2C_RecvByte();
void I2C_ReadPage();
void I2C_WritePage();
void display_ACCEL_x();
void display_ACCEL_y();
void display_ACCEL_z();
uchar Single_ReadI2C(uchar REG_Address); //读取I2C数据
void Single_WriteI2C(uchar REG_Address,uchar REG_data); //向I2C写入数据
//********************************************************************************
//整数转字符串
//********************************************************************************
void lcd_printf(uchar *s,int temp_data)
{
if(temp_data<0)
{
temp_data=-temp_data;
*s='-';
}
else *s=' ';
*++s =temp_data/10000+0x30;
temp_data=temp_data%10000; //取余运算
*++s =temp_data/1000+0x30;
temp_data=temp_data%1000; //取余运算
*++s =temp_data/100+0x30;
temp_data=temp_data%100; //取余运算
*++s =temp_data/10+0x30;
temp_data=temp_data%10; //取余运算
*++s =temp_data+0x30;
}
//******************************************************************************************************
//串口初始化
//*******************************************************************************************************
void init_uart()
{
TMOD=0x21;
TH1=0xfd; //实现波特率9600（系统时钟11.0592MHZ）
TL1=0xfd;
SCON=0x50;
PS=1; //串口中断设为高优先级别
TR0=1; //启动定时器
TR1=1;
ET0=1; //打开定时器0中断
ES=1;
EA=1;
}
//*************************************************************************************************
//串口发送函数
//*************************************************************************************************
void SeriPushSend(uchar send_data)
{
SBUF=send_data;
while(!TI);TI=0;
}
//*************************************************************************************************
//************************************延时*********************************************************
//*************************************************************************************************
void delay(unsigned int k)
{
unsigned int i,j;
for(i=0;i<k;i++)
{
for(j=0;j<121;j++);
}
}
//************************************************************************************************
//延时5微秒(STC90C52RC@12M)
//不同的工作环境,需要调整此函数
//注意当改用1T的MCU时,请调整此延时函数
//************************************************************************************************
void Delay5us()
{
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
}
//*************************************************************************************************
//I2C起始信号
//*************************************************************************************************
void I2C_Start()
{
SDA = 1; //拉高数据线
SCL = 1; //拉高时钟线
Delay5us(); //延时
SDA = 0; //产生下降沿
Delay5us(); //延时
SCL = 0; //拉低时钟线
}
//*************************************************************************************************
//I2C停止信号
//*************************************************************************************************
void I2C_Stop()
{
SDA = 0; //拉低数据线
SCL = 1; //拉高时钟线
Delay5us(); //延时
SDA = 1; //产生上升沿
Delay5us(); //延时
}
//**************************************************************************************************
//I2C发送应答信号
//入口参数:ack (0:ACK 1:NAK)
//**************************************************************************************************
void I2C_SendACK(bit ack)
{
SDA = ack; //写应答信号
SCL = 1; //拉高时钟线
Delay5us(); //延时
SCL = 0; //拉低时钟线
Delay5us(); //延时
}
//****************************************************************************************************
//I2C接收应答信号
//****************************************************************************************************
bit I2C_RecvACK()
{
SCL = 1; //拉高时钟线
Delay5us(); //延时
CY = SDA; //读应答信号
SCL = 0; //拉低时钟线
Delay5us(); //延时
return CY;
}
//*****************************************************************************************************
//向I2C总线发送一个字节数据
//*****************************************************************************************************
void I2C_SendByte(uchar dat)
{
uchar i;
for (i=0; i<8; i++) //8位计数器
{
dat <<= 1; //移出数据的最高位
SDA = CY; //送数据口
SCL = 1; //拉高时钟线
Delay5us(); //延时
SCL = 0; //拉低时钟线
Delay5us(); //延时
}
I2C_RecvACK();
}
//*****************************************************************************************************
//从I2C总线接收一个字节数据
//******************************************************************************************************
uchar I2C_RecvByte()
{
uchar i;
uchar dat = 0;
SDA = 1; //使能内部上拉,准备读取数据,
for (i=0; i<8; i++) //8位计数器
{
dat <<= 1;
SCL = 1; //拉高时钟线
Delay5us(); //延时
dat |= SDA; //读数据
SCL = 0; //拉低时钟线
Delay5us(); //延时
}
return dat;
}
//*****************************************************************************************************
//向I2C设备写入一个字节数据
//*****************************************************************************************************
void Single_WriteI2C(uchar REG_Address,uchar REG_data)
{
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress); //发送设备地址+写信号
I2C_SendByte(REG_Address); //内部寄存器地址，
I2C_SendByte(REG_data); //内部寄存器数据，
I2C_Stop(); //发送停止信号
}
//*******************************************************************************************************
//从I2C设备读取一个字节数据
//*******************************************************************************************************
uchar Single_ReadI2C(uchar REG_Address)
{
uchar REG_data;
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress); //发送设备地址+写信号
I2C_SendByte(REG_Address); //发送存储单元地址，从0开始
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress+1); //发送设备地址+读信号
REG_data=I2C_RecvByte(); //读出寄存器数据
I2C_SendACK(1); //接收应答信号
I2C_Stop(); //停止信号
return REG_data;
}
//******************************************************************************************************
//初始化MPU6050
//******************************************************************************************************
void InitMPU6050()
{
Single_WriteI2C(PWR_MGMT_1, 0x00); //解除休眠状态
Single_WriteI2C(SMPLRT_DIV, 0x07);
Single_WriteI2C(CONFIG, 0x06);
Single_WriteI2C(GYRO_CONFIG, 0x18);
Single_WriteI2C(ACCEL_CONFIG, 0x01);
}
//******************************************************************************************************
//合成数据
//******************************************************************************************************
int GetData(uchar REG_Address)
{
uchar H,L;
H=Single_ReadI2C(REG_Address);
L=Single_ReadI2C(REG_Address+1);
return ((H<<8)+L); //合成数据
}
//******************************************************************************************************
//超级终端（串口调试助手）上显示10位数据
//******************************************************************************************************
void Display10BitData(int value)
{ uchar i;
// value/=64; //转换为10位数据
lcd_printf(dis, value); //转换数据显示
for(i=0;i<6;i++)
{
SeriPushSend(dis[i]);
}
// DisplayListChar(x,y,dis,4); //启始列，行，显示数组，显示长度
}
//*********************************************************
// 卡尔曼滤波
//*********************************************************
//Kalman滤波，20MHz的处理时间约0.77ms；
float Kalman_Filter(float Accel,float Gyro)
{
static const float Q_angle=0.001;
static const float Q_gyro=0.003;
static const float R_angle=0.5;
static const float dt=0.01; //dt为kalman滤波器采样时间;
static const char C_0 = 1;
static float Q_bias, Angle_err;
static float PCt_0, PCt_1, E;
static float K_0, K_1, t_0, t_1;
static float Pdot[4] ={0,0,0,0};
static float PP[2][2] = { { 1, 0 },{ 0, 1 } };
Angle+=(Gyro - Q_bias) * dt; //先验估计
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]=- PP[1][1];
Pdot[2]=- PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle;
//zk-先验估计
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;
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_y = Gyro - Q_bias;
//输出值(后验估计)的微分=角速度
return Gyro_y;
}
//*********************************************************
// 倾角计算（卡尔曼融合）
//*********************************************************
void Angle_Calcu(void)
{
//------加速度--------------------------
//范围为2g时，换算关系：16384 LSB/g
//角度较小时，x=sinx得到角度（弧度）, deg = rad*180/3.14
//因为x>=sinx,故乘以1.3适当放大
Accel_x = GetData(ACCEL_XOUT_H);
//读取X轴加速度
Angle_ax = (Accel_x - 1100) /16384; //去除零点偏移,计算得到角度（弧度）
Angle_ax = Angle_ax*1.4*180/3.14; //弧度转换为度,
//Display10BitData(Angle_ax,2,1);
//-------角速度-------------------------
//范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)
Gyro_y = GetData(GYRO_YOUT_H);
//静止时角速度Y轴输出为-30左右
Gyro_y = -(Gyro_y + 30)/16.4; //去除零点偏移，计算角速度值,负号为方向处理
//Angle_gy = Angle_gy + Gyro_y*0.01; //角速度积分得到倾斜角度.
//Display10BitData(Gyro_y,8,1);
//-------卡尔曼滤波融合-----------------------
//Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角
Display10BitData(Kalman_Filter(Angle_ax,Gyro_y));
/*//-------互补滤波-----------------------
//补偿原理是取当前倾角和加速度获得倾角差值进行放大，然后与
//陀螺仪角速度叠加后再积分，从而使倾角最跟踪为加速度获得的角度
//0.5为放大倍数，可调节补偿度；0.01为系统周期10ms
Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);*/
}
//*******************************************************************************************************
//主程序
//*******************************************************************************************************
void main()
{
delay(500); //上电延时
init_uart();
InitMPU6050(); //初始化MPU6050
delay(150);
while(1)
{
Angle_Calcu();
SeriPushSend(0x0d);
SeriPushSend(0x0a);//换行，回车
delay(2000);
}
}
我觉的这次代码问题不大了啊，为什么获取的值为 -00001 还是不对。
继续思考！！
四、可能出现错误：
Rebuild target 'Target 1'
assembling STARTUP.A51...
compiling MPU6050_C52.c...
linking...
*** WARNING L16: UNCALLED SEGMENT, IGNORED FOR OVERLAY PROCESS
SEGMENT: ?PR?_DISPLAY10BITDATA?MPU6050_C52
*** WARNING L16: UNCALLED SEGMENT, IGNORED FOR OVERLAY PROCESS
SEGMENT: ?PR?_KALMAN_FILTER?MPU6050_C52
*** WARNING L16: UNCALLED SEGMENT, IGNORED FOR OVERLAY PROCESS
SEGMENT: ?PR?ANGLE_CALCULATE?MPU6050_C52
*** ERROR L107: ADDRESS SPACE OVERFLOW
SPACE: DATA
SEGMENT: ?DT?MPU6050_C52
LENGTH: 0071H
*** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENT
SYMBOL: GYRO_Y
SEGMENT: ?DT?MPU6050_C52
*** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENT
SYMBOL: ANGLE
SEGMENT: ?DT?MPU6050_C52
*** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENT
SYMBOL: DIS
SEGMENT: ?DT?MPU6050_C52
*** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENT
SYMBOL: DIS_DATA
SEGMENT: ?DT?MPU6050_C52
*** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENT
SYMBOL: ACCEL_X
SEGMENT: ?DT?MPU6050_C52
*** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENT
SYMBOL: ANGLE_GY
SEGMENT: ?DT?MPU6050_C52
*** ERROR L105: PUBLIC REFERS TO IGNORED SEGMENT
SYMBOL: ANGLE_AX
SEGMENT: ?DT?MPU6050_C52
Program Size: data=135.1 xdata=0 code=2873
Target not created说明data空间已经不够用，原因是你可能有好多函数，而函数内部的局部变量又没有定义其空间。
这种情况下，系统会将变量分配到你在 Otions for Target 对话框里的设置的空间。
如果你在下图所示中的 Memory Model 里设置成 Small:variables in DATA,则DATA空间很快便用完，导致data空间不够用。
解决的办法有两种：
一是通过更改Memory Model设置，可以设置成pdata或xdata，以便有足够大的空间。
但这又带来新的问题，程序运行速度减慢，而且code代码也会加大，因为如果一个局部变量被存放在了xdata空间，汇编语言访问xdata空间的代码大小要比访问data空间的代码大，变量一旦很多，程序的代码也会逐渐增大；
二是根据自己的要求设置变量的空间。
所以这涉及到代码优化的问题，遇到具体问题时，在运行速度和代码大小之间取得适合自己的情况。