基于Kalman滤波的姿态传感器算法代码

把加速度传感器、陀螺传感器、磁罗盘传感器的3D采样值，通过Kalman滤波算法实现参数整合，得到3D姿态参数。

/*
 * i2ccomm.cpp
 *
 * Created: 16/12/2011 16:52:58
 *  Author: Lo颿 KAEMMERLEN
 */ 

#include “i2ccomm.h“

#include   
#include 
#include 
#include 
#include 	

void i2c_start（） {  
	TWCR = （1 << TWINT） | （1 << TWSTA） | （1 << TWEN）; // send start condition  
	while （!（TWCR & （1 << TWINT）））;  
}  
  
void i2c_write_byte（char byte） {  
	TWDR = byte;              
	TWCR = （1 << TWINT） | （1 << TWEN）; // start address transmission  
	while （!（TWCR & （1 << TWINT）））;  
}  
  
char i2c_read_byte（） {  
	TWCR = （1 << TWINT） | （1 << TWEA） | （1 << TWEN）; // start data reception transmit ACK  
	while （!（TWCR & （1 << TWINT）））;  
	return TWDR;  
}  

char i2c_read_last_byte（） {  
	TWCR = （1 << TWINT） | （1 << TWEN）; // start data reception
	while （!（TWCR & （1 << TWINT）））;  
	return TWDR;  
}  
  
void i2c_stop（） {  
	  TWCR = （1 << TWINT） | （1 << TWSTO） | （1 << TWEN）; // send stop condition  
}  

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2012-02-17 06:35  KalmanFilteredIMU\
     文件         654  2012-02-17 06:42  KalmanFilteredIMU\IMPORTANT TO BUILD.txt
     目录           0  2012-02-17 06:35  KalmanFilteredIMU\KalmanFilteredIMU\
     文件         924  2012-02-17 06:32  KalmanFilteredIMU\KalmanFilteredIMU.avrsln
     文件       22528  2012-02-17 06:44  KalmanFilteredIMU\KalmanFilteredIMU.avrsuo
     目录           0  2012-02-17 06:48  KalmanFilteredIMU\KalmanFilteredIMU\Debug\
     文件        4347  2012-02-17 06:42  KalmanFilteredIMU\KalmanFilteredIMU\Debug\i2ccomm.d
     文件        4180  2012-02-17 06:42  KalmanFilteredIMU\KalmanFilteredIMU\Debug\i2ccomm.o
     文件        4950  2012-02-17 06:42  KalmanFilteredIMU\KalmanFilteredIMU\Debug\KalmanFilter.d
     文件       21356  2012-02-17 06:42  KalmanFilteredIMU\KalmanFilteredIMU\Debug\KalmanFilter.o
     文件        4733  2012-02-17 06:48  KalmanFilteredIMU\KalmanFilteredIMU\Debug\KalmanFilteredIMU.d
     文件          13  2012-02-17 06:48  KalmanFilteredIMU\KalmanFilteredIMU\Debug\KalmanFilteredIMU.eep
     文件       86154  2012-02-17 06:48  KalmanFilteredIMU\KalmanFilteredIMU\Debug\KalmanFilteredIMU.elf
     文件       73204  2012-02-17 06:48  KalmanFilteredIMU\KalmanFilteredIMU\Debug\KalmanFilteredIMU.hex
     文件      559649  2012-02-17 06:48  KalmanFilteredIMU\KalmanFilteredIMU\Debug\KalmanFilteredIMU.lss
     文件       82085  2012-02-17 06:48  KalmanFilteredIMU\KalmanFilteredIMU\Debug\KalmanFilteredIMU.map
     文件       17744  2012-02-17 06:48  KalmanFilteredIMU\KalmanFilteredIMU\Debug\KalmanFilteredIMU.o
     文件         336  2012-02-17 06:35  KalmanFilteredIMU\KalmanFilteredIMU\Debug\makedep.mk
     文件        4561  2012-02-17 06:42  KalmanFilteredIMU\KalmanFilteredIMU\Debug\Makefile
     文件        2274  2012-02-17 06:42  KalmanFilteredIMU\KalmanFilteredIMU\Debug\mathstools.d
     文件       20832  2012-02-17 06:42  KalmanFilteredIMU\KalmanFilteredIMU\Debug\mathstools.o
     文件        4925  2012-02-17 06:42  KalmanFilteredIMU\KalmanFilteredIMU\Debug\sensors.d
     文件       13136  2012-02-17 06:42  KalmanFilteredIMU\KalmanFilteredIMU\Debug\sensors.o
     文件        4055  2012-02-17 06:42  KalmanFilteredIMU\KalmanFilteredIMU\Debug\usart.d
     文件       13328  2012-02-17 06:42  KalmanFilteredIMU\KalmanFilteredIMU\Debug\usart.o
     文件         988  2012-02-17 04:49  KalmanFilteredIMU\KalmanFilteredIMU\i2ccomm.cpp
     文件         284  2012-02-17 04:49  KalmanFilteredIMU\KalmanFilteredIMU\i2ccomm.h
     文件        7151  2012-02-17 08:21  KalmanFilteredIMU\KalmanFilteredIMU\KalmanFilter.cpp
     文件        1355  2012-02-17 05:36  KalmanFilteredIMU\KalmanFilteredIMU\KalmanFilter.h
     文件        6408  2012-02-17 06:39  KalmanFilteredIMU\KalmanFilteredIMU\KalmanFilteredIMU.avrgccproj
     文件        6224  2012-02-17 06:48  KalmanFilteredIMU\KalmanFilteredIMU\KalmanFilteredIMU.cpp
............此处省略6个文件信息