IMU_demo - | Mbed

Users » higedura » Code » IMU_demo

main.cpp@0:f5a5a0e5fefb, 2012-04-23 (annotated)

Committer:: higedura
Date:: Mon Apr 23 14:59:48 2012 +0000
Revision:: 0:f5a5a0e5fefb

Who changed what in which revision?

User	Revision	Line number	New contents of line
higedura	0:f5a5a0e5fefb	1	// IMU for Sparkfun 9DOF Sensor Stick
higedura	0:f5a5a0e5fefb	2
higedura	0:f5a5a0e5fefb	3	#include "mbed.h"
higedura	0:f5a5a0e5fefb	4	#include "ADXL345_I2C.h"
higedura	0:f5a5a0e5fefb	5	#include "ITG3200.h"
higedura	0:f5a5a0e5fefb	6	#include "HMC5883L.h"
higedura	0:f5a5a0e5fefb	7	#include "TextLCD.h"
higedura	0:f5a5a0e5fefb	8
higedura	0:f5a5a0e5fefb	9	DigitalOut led1(LED1);
higedura	0:f5a5a0e5fefb	10	DigitalOut led2(LED2);
higedura	0:f5a5a0e5fefb	11	DigitalOut led3(LED3);
higedura	0:f5a5a0e5fefb	12	DigitalOut led4(LED4);
higedura	0:f5a5a0e5fefb	13	ADXL345_I2C accelerometer(p9, p10);
higedura	0:f5a5a0e5fefb	14	ITG3200 gyro(p9, p10);
higedura	0:f5a5a0e5fefb	15	HMC5883L compass(p9, p10);
higedura	0:f5a5a0e5fefb	16	TextLCD lcd(p24, p26, p27, p28, p29, p30);
higedura	0:f5a5a0e5fefb	17	Serial pc(USBTX, USBRX);
higedura	0:f5a5a0e5fefb	18
higedura	0:f5a5a0e5fefb	19	#define pi 3.14159265
higedura	0:f5a5a0e5fefb	20
higedura	0:f5a5a0e5fefb	21	#define N 3
higedura	0:f5a5a0e5fefb	22	#define N_LWMA 10
higedura	0:f5a5a0e5fefb	23
higedura	0:f5a5a0e5fefb	24	double* RK4( double, double[N], double[N] );
higedura	0:f5a5a0e5fefb	25	double* func( double[N], double[N] );
higedura	0:f5a5a0e5fefb	26	double* LWMA( double[N] );
higedura	0:f5a5a0e5fefb	27	double* EKF_predict( double[N], double[N] );
higedura	0:f5a5a0e5fefb	28	double* EKF_correct( double[N], double[N], double[N] );
higedura	0:f5a5a0e5fefb	29
higedura	0:f5a5a0e5fefb	30	// 0 1 2
higedura	0:f5a5a0e5fefb	31	// [ accXn-1 accXn-2 ... ] 0
higedura	0:f5a5a0e5fefb	32	// zBuf = [ accYn-1 accYn-2 ... ] 1
higedura	0:f5a5a0e5fefb	33	// [ accZn-1 accZn-2 ... ] 2
higedura	0:f5a5a0e5fefb	34	double z_buf[N][N_LWMA] = {0}; // For LWMA
higedura	0:f5a5a0e5fefb	35
higedura	0:f5a5a0e5fefb	36	double P[N][N] = {{1,0,0},{0,1,0},{0,0,1}}; // For EKF
higedura	0:f5a5a0e5fefb	37
higedura	0:f5a5a0e5fefb	38	int main(){
higedura	0:f5a5a0e5fefb	39
higedura	0:f5a5a0e5fefb	40	pc.baud(921600);
higedura	0:f5a5a0e5fefb	41
higedura	0:f5a5a0e5fefb	42	// loop
higedura	0:f5a5a0e5fefb	43	int correct_loop = 0;
higedura	0:f5a5a0e5fefb	44	int calib_loop = 1000;
higedura	0:f5a5a0e5fefb	45	int lcd_loop = 0;
higedura	0:f5a5a0e5fefb	46
higedura	0:f5a5a0e5fefb	47	//double dt_wait = 0.00514;
higedura	0:f5a5a0e5fefb	48	double dt_wait = 0.0046;
higedura	0:f5a5a0e5fefb	49	double dt = 0.01;
higedura	0:f5a5a0e5fefb	50	double t = 0;
higedura	0:f5a5a0e5fefb	51
higedura	0:f5a5a0e5fefb	52	int bit_acc[N] = {0}; // Buffer of the accelerometer
higedura	0:f5a5a0e5fefb	53	int get_mag[N] = {0}; // Buffer of the compass
higedura	0:f5a5a0e5fefb	54
higedura	0:f5a5a0e5fefb	55	// Calibration routine
higedura	0:f5a5a0e5fefb	56	double calib_acc[N] = {0};
higedura	0:f5a5a0e5fefb	57	double calib_gyro_buf[N] = {0};
higedura	0:f5a5a0e5fefb	58	double calib_gyro[N] = {0};
higedura	0:f5a5a0e5fefb	59	double compass_basis_buf[N] = {0};
higedura	0:f5a5a0e5fefb	60	double compass_basis_rad = 0;
higedura	0:f5a5a0e5fefb	61
higedura	0:f5a5a0e5fefb	62	// Getting data
higedura	0:f5a5a0e5fefb	63	double acc[N] = {0};
higedura	0:f5a5a0e5fefb	64	double gyro_deg[N] = {0};
higedura	0:f5a5a0e5fefb	65	double gyro_rad[N] = {0};
higedura	0:f5a5a0e5fefb	66	int mag[N] = {0};
higedura	0:f5a5a0e5fefb	67
higedura	0:f5a5a0e5fefb	68	// Measurement algorithm
higedura	0:f5a5a0e5fefb	69	double ang_acc[2] = {0};
higedura	0:f5a5a0e5fefb	70	double ang_deg[N] = {0};
higedura	0:f5a5a0e5fefb	71	double ang_rad[N] = {0};
higedura	0:f5a5a0e5fefb	72	double zLWMA[N] = {0};
higedura	0:f5a5a0e5fefb	73	double compass_rad = 0;
higedura	0:f5a5a0e5fefb	74	double compass_deg = 0;
higedura	0:f5a5a0e5fefb	75
higedura	0:f5a5a0e5fefb	76	for( int i=0;i<N_LWMA;i++ ){
higedura	0:f5a5a0e5fefb	77	z_buf[2][i] = 1;
higedura	0:f5a5a0e5fefb	78	}
higedura	0:f5a5a0e5fefb	79
higedura	0:f5a5a0e5fefb	80	double* p_ang_RK4;
higedura	0:f5a5a0e5fefb	81	double* p_ang_EKF;
higedura	0:f5a5a0e5fefb	82	double* p_zLWMA_buf;
higedura	0:f5a5a0e5fefb	83
higedura	0:f5a5a0e5fefb	84	// * Setting up accelerometer *
higedura	0:f5a5a0e5fefb	85	// These are here to test whether any of the initialization fails. It will print the failure.
higedura	0:f5a5a0e5fefb	86	if(accelerometer.setPowerControl(0x00)){
higedura	0:f5a5a0e5fefb	87	pc.printf("didn't intitialize power control\n\r");
higedura	0:f5a5a0e5fefb	88	return 0;
higedura	0:f5a5a0e5fefb	89	}
higedura	0:f5a5a0e5fefb	90	// Full resolution, +/-16g, 4mg/LSB.
higedura	0:f5a5a0e5fefb	91	wait(.001);
higedura	0:f5a5a0e5fefb	92
higedura	0:f5a5a0e5fefb	93	if(accelerometer.setDataFormatControl(0x0B)){
higedura	0:f5a5a0e5fefb	94	pc.printf("didn't set data format\n\r");
higedura	0:f5a5a0e5fefb	95	return 0; }
higedura	0:f5a5a0e5fefb	96	wait(.001);
higedura	0:f5a5a0e5fefb	97
higedura	0:f5a5a0e5fefb	98	// 3.2kHz data rate.
higedura	0:f5a5a0e5fefb	99	if(accelerometer.setDataRate(ADXL345_3200HZ)){
higedura	0:f5a5a0e5fefb	100	pc.printf("didn't set data rate\n\r");
higedura	0:f5a5a0e5fefb	101	return 0;
higedura	0:f5a5a0e5fefb	102	}
higedura	0:f5a5a0e5fefb	103	wait(.001);
higedura	0:f5a5a0e5fefb	104
higedura	0:f5a5a0e5fefb	105	if(accelerometer.setPowerControl(MeasurementMode)) {
higedura	0:f5a5a0e5fefb	106	pc.printf("didn't set the power control to measurement\n\r");
higedura	0:f5a5a0e5fefb	107	return 0;
higedura	0:f5a5a0e5fefb	108	}
higedura	0:f5a5a0e5fefb	109	// * Setting up accelerometer *
higedura	0:f5a5a0e5fefb	110
higedura	0:f5a5a0e5fefb	111	// * Setting up gyro *
higedura	0:f5a5a0e5fefb	112	gyro.setLpBandwidth(LPFBW_42HZ);
higedura	0:f5a5a0e5fefb	113
higedura	0:f5a5a0e5fefb	114	// * Setting up compass *
higedura	0:f5a5a0e5fefb	115	compass.setDefault();
higedura	0:f5a5a0e5fefb	116
higedura	0:f5a5a0e5fefb	117	// Wait some time for all sensors (Need at least 5ms)
higedura	0:f5a5a0e5fefb	118	wait(0.1);
higedura	0:f5a5a0e5fefb	119
higedura	0:f5a5a0e5fefb	120	// * Calibration routine *
higedura	0:f5a5a0e5fefb	121	pc.printf("\n\rDon't touch! Calibrating now!\n\r");
higedura	0:f5a5a0e5fefb	122	lcd.locate(0, 0); lcd.printf("Don't touch!");
higedura	0:f5a5a0e5fefb	123	lcd.locate(0, 1); lcd.printf("Calibrating now!");
higedura	0:f5a5a0e5fefb	124	led1 = 1;
higedura	0:f5a5a0e5fefb	125	for( int i=0;i<calib_loop;i++ ){
higedura	0:f5a5a0e5fefb	126	accelerometer.getOutput(bit_acc);
higedura	0:f5a5a0e5fefb	127	compass.readData(get_mag);
higedura	0:f5a5a0e5fefb	128	calib_gyro_buf[0] = gyro.getGyroX();
higedura	0:f5a5a0e5fefb	129	calib_gyro_buf[1] = gyro.getGyroY();
higedura	0:f5a5a0e5fefb	130	calib_gyro_buf[2] = gyro.getGyroZ();
higedura	0:f5a5a0e5fefb	131	for( int j=0;j<N;j++ ){
higedura	0:f5a5a0e5fefb	132	calib_acc[j] += (int16_t)bit_acc[j];
higedura	0:f5a5a0e5fefb	133	calib_gyro[j] += calib_gyro_buf[j];
higedura	0:f5a5a0e5fefb	134	compass_basis_buf[j] += (int16_t)get_mag[j];
higedura	0:f5a5a0e5fefb	135	}
higedura	0:f5a5a0e5fefb	136	if( i>calib_loop*3/4 ){
higedura	0:f5a5a0e5fefb	137	led4 = 1;
higedura	0:f5a5a0e5fefb	138	}else if( i>calib_loop/2 ){
higedura	0:f5a5a0e5fefb	139	led3 = 1;
higedura	0:f5a5a0e5fefb	140	}else if( i>calib_loop/4 ){
higedura	0:f5a5a0e5fefb	141	led2 = 1;
higedura	0:f5a5a0e5fefb	142	}
higedura	0:f5a5a0e5fefb	143	}
higedura	0:f5a5a0e5fefb	144	for( int i=0;i<N;i++ ){
higedura	0:f5a5a0e5fefb	145	calib_acc[i] = calib_acc[i]/calib_loop;
higedura	0:f5a5a0e5fefb	146	calib_gyro[i] = calib_gyro[i]/calib_loop;
higedura	0:f5a5a0e5fefb	147	compass_basis_buf[i] = compass_basis_buf[i]/calib_loop;
higedura	0:f5a5a0e5fefb	148	}
higedura	0:f5a5a0e5fefb	149	compass_basis_rad = compass_basis_buf[1]/compass_basis_buf[0];
higedura	0:f5a5a0e5fefb	150	compass_basis_rad = atan(compass_basis_rad);
higedura	0:f5a5a0e5fefb	151	led1 = 0; led2 = 0; led3 = 0; led4 = 0;
higedura	0:f5a5a0e5fefb	152	pc.printf("accX accY accZ gyroX gyroY gyroZ yaw_basis\n\r");
higedura	0:f5a5a0e5fefb	153	pc.printf("%f, %f, %f, %f, %f, %f, %f\n\r",calib_acc[0],calib_acc[1],calib_acc[2],calib_gyro[0],calib_gyro[1],calib_gyro[2],compass_basis_rad*180/pi);
higedura	0:f5a5a0e5fefb	154	lcd.cls();
higedura	0:f5a5a0e5fefb	155	lcd.locate(0, 0); lcd.printf("Starting IMU");
higedura	0:f5a5a0e5fefb	156	lcd.locate(0, 1); lcd.printf("IMU with EKF");
higedura	0:f5a5a0e5fefb	157	for( int i=0;i<3;i++ ){
higedura	0:f5a5a0e5fefb	158	wait(0.5);
higedura	0:f5a5a0e5fefb	159	led1 = 1; led2 = 1; led3 = 1; led4 = 1;
higedura	0:f5a5a0e5fefb	160	wait(0.5);
higedura	0:f5a5a0e5fefb	161	led1 = 0; led2 = 0; led3 = 0; led4 = 0;
higedura	0:f5a5a0e5fefb	162	}
higedura	0:f5a5a0e5fefb	163	// * Calibration routine *
higedura	0:f5a5a0e5fefb	164
higedura	0:f5a5a0e5fefb	165	lcd.cls();
higedura	0:f5a5a0e5fefb	166	pc.printf("Starting IMU unit \n\r");
higedura	0:f5a5a0e5fefb	167	pc.printf(" Time AccX AccY AccZ GyroX GyroY GyroZ MagX MagY MagZ\n\r");
higedura	0:f5a5a0e5fefb	168	while( 1 ){
higedura	0:f5a5a0e5fefb	169	//for( int i=0;i<10000;i++ ){
higedura	0:f5a5a0e5fefb	170
higedura	0:f5a5a0e5fefb	171	// Updating accelerometer and compass
higedura	0:f5a5a0e5fefb	172	accelerometer.getOutput(bit_acc);
higedura	0:f5a5a0e5fefb	173	compass.readData(get_mag);
higedura	0:f5a5a0e5fefb	174
higedura	0:f5a5a0e5fefb	175	// Transfering units (Acc[g], Gyro[deg/s], Compass[Ga])
higedura	0:f5a5a0e5fefb	176	acc[0] = -((int16_t)bit_acc[0]-calib_acc[0])*0.004;
higedura	0:f5a5a0e5fefb	177	acc[1] = ((int16_t)bit_acc[1]-calib_acc[1])*0.004;
higedura	0:f5a5a0e5fefb	178	acc[2] = ((int16_t)bit_acc[2]-calib_acc[2])*0.004+1;
higedura	0:f5a5a0e5fefb	179
higedura	0:f5a5a0e5fefb	180	gyro_deg[0] = (gyro.getGyroX()-calib_gyro[0])/14.375;
higedura	0:f5a5a0e5fefb	181	gyro_deg[1] = (gyro.getGyroY()-calib_gyro[1])/14.375;
higedura	0:f5a5a0e5fefb	182	gyro_deg[2] = (gyro.getGyroZ()-calib_gyro[2])/14.375;
higedura	0:f5a5a0e5fefb	183
higedura	0:f5a5a0e5fefb	184	for( int i=0;i<N;i++ ){ mag[0] = (int16_t)get_mag[0]; }
higedura	0:f5a5a0e5fefb	185
higedura	0:f5a5a0e5fefb	186	// Low pass filter for acc
higedura	0:f5a5a0e5fefb	187	//for( int i=0;i<N;i++ ){ if( -0.05<acc[i] && acc[i]<0.05 ){ acc[i] = 0; } }
higedura	0:f5a5a0e5fefb	188
higedura	0:f5a5a0e5fefb	189	for( int i=0;i<N;i++ ){ if( acc[i]<-2 ){ acc[i] = -2; } }
higedura	0:f5a5a0e5fefb	190	for( int i=0;i<N;i++ ){ if( acc[i]>2 ){ acc[i] = 2; } }
higedura	0:f5a5a0e5fefb	191
higedura	0:f5a5a0e5fefb	192	// Low pass filter for gyro
higedura	0:f5a5a0e5fefb	193	//for( int i=0;i<N;i++ ){ if( -0.5<gyro_deg[i] && gyro_deg[i]<0.5 ){ gyro_deg[i] = 0; } }
higedura	0:f5a5a0e5fefb	194
higedura	0:f5a5a0e5fefb	195	for( int i=0;i<N;i++ ){ gyro_rad[i] = gyro_deg[i]*pi/180; }
higedura	0:f5a5a0e5fefb	196
higedura	0:f5a5a0e5fefb	197	// Compass yaw
higedura	0:f5a5a0e5fefb	198	/*compass_rad = (double)mag[1]/mag[0];
higedura	0:f5a5a0e5fefb	199	compass_rad = atan(compass_rad);
higedura	0:f5a5a0e5fefb	200	//compass_rad = compass_rad-compass_basis_rad;
higedura	0:f5a5a0e5fefb	201	compass_deg = compass_rad*180/pi;
higedura	0:f5a5a0e5fefb	202	*/
higedura	0:f5a5a0e5fefb	203	// LWMA (Observation)
higedura	0:f5a5a0e5fefb	204	p_zLWMA_buf = LWMA(acc);
higedura	0:f5a5a0e5fefb	205	for( int i=0;i<N;i++ ){ zLWMA[i] = *p_zLWMA_buf; p_zLWMA_buf = p_zLWMA_buf+1; }
higedura	0:f5a5a0e5fefb	206	// LWMA angle
higedura	0:f5a5a0e5fefb	207	ang_acc[0] = asin(zLWMA[1])*180/pi;
higedura	0:f5a5a0e5fefb	208	ang_acc[1] = asin(zLWMA[0])*180/pi;
higedura	0:f5a5a0e5fefb	209
higedura	0:f5a5a0e5fefb	210	// RK4 (Prediction)
higedura	0:f5a5a0e5fefb	211	p_ang_RK4 = RK4(dt,ang_rad,gyro_rad);
higedura	0:f5a5a0e5fefb	212	for( int i=0;i<N;i++ ){ ang_rad[i] = *p_ang_RK4; p_ang_RK4 = p_ang_RK4+1; }
higedura	0:f5a5a0e5fefb	213
higedura	0:f5a5a0e5fefb	214	// EKF (Correction)
higedura	0:f5a5a0e5fefb	215	EKF_predict(ang_rad,gyro_rad);
higedura	0:f5a5a0e5fefb	216	correct_loop++;
higedura	0:f5a5a0e5fefb	217	if ( correct_loop>=10 ){
higedura	0:f5a5a0e5fefb	218	p_ang_EKF = EKF_correct(ang_rad,gyro_rad,zLWMA);
higedura	0:f5a5a0e5fefb	219	for ( int i=0;i<N;i++ ){ ang_deg[i] = *p_ang_EKF; p_ang_EKF = p_ang_EKF+1; }
higedura	0:f5a5a0e5fefb	220	correct_loop = 0;
higedura	0:f5a5a0e5fefb	221	}
higedura	0:f5a5a0e5fefb	222	for( int i=0;i<N;i++ ){ ang_deg[i] = 1.76ang_rad[i]180/pi; }
higedura	0:f5a5a0e5fefb	223
higedura	0:f5a5a0e5fefb	224	//pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2], mag[0], mag[1], mag[2]);
higedura	0:f5a5a0e5fefb	225	//pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2]);
higedura	0:f5a5a0e5fefb	226	//pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, gyro_deg[0], gyro_deg[1], gyro_deg[2], ang_deg[0], ang_deg[1], ang_deg[2]);
higedura	0:f5a5a0e5fefb	227	//pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f\n\r", t, acc[0], acc[1], acc[2], zLWMA[0], zLWMA[1], zLWMA[2]);
higedura	0:f5a5a0e5fefb	228	//pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f\n\r", t, zLWMA[0], zLWMA[1], zLWMA[2], ang_acc[0], ang_acc[1]);
higedura	0:f5a5a0e5fefb	229	//pc.printf("%d, %d, %f\n\r",mag[0], mag[1], compass_deg);
higedura	0:f5a5a0e5fefb	230	//pc.printf("%7.2f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, ang_acc[0], ang_acc[1], ang_deg[0], ang_deg[1], ang_deg[2]);
higedura	0:f5a5a0e5fefb	231	pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, gyro_deg[0], gyro_deg[1], gyro_deg[2], ang_deg[0], ang_deg[1], ang_deg[2]);
higedura	0:f5a5a0e5fefb	232	//pc.printf("%7.2f, %7.1f, %7.1f, 0.0\n\r", t, ang_deg[0], ang_deg[1]);
higedura	0:f5a5a0e5fefb	233	lcd_loop++;
higedura	0:f5a5a0e5fefb	234	if( lcd_loop>=20 ){
higedura	0:f5a5a0e5fefb	235	lcd.locate(0, 0); lcd.printf("phi:%6.1f [deg]", ang_deg[0]);
higedura	0:f5a5a0e5fefb	236	lcd.locate(0, 1); lcd.printf("the:%6.1f [deg]", ang_deg[1]);
higedura	0:f5a5a0e5fefb	237	lcd_loop = 0;
higedura	0:f5a5a0e5fefb	238	}
higedura	0:f5a5a0e5fefb	239
higedura	0:f5a5a0e5fefb	240	wait(dt_wait);
higedura	0:f5a5a0e5fefb	241	t += dt;
higedura	0:f5a5a0e5fefb	242
higedura	0:f5a5a0e5fefb	243	}
higedura	0:f5a5a0e5fefb	244
higedura	0:f5a5a0e5fefb	245	led1 = 1; led2 = 1; led3 = 1; led4 = 1;
higedura	0:f5a5a0e5fefb	246
higedura	0:f5a5a0e5fefb	247	}
higedura	0:f5a5a0e5fefb	248
higedura	0:f5a5a0e5fefb	249
higedura	0:f5a5a0e5fefb	250	double* EKF_predict( double y[N], double x[N] ){
higedura	0:f5a5a0e5fefb	251	// x = F * x;
higedura	0:f5a5a0e5fefb	252	// P = F * P * F' + G * Q * G';
higedura	0:f5a5a0e5fefb	253
higedura	0:f5a5a0e5fefb	254	//double Q[N][N] = { {0.1, 0, 0}, {0, 0.1, 0}, {0, 0, 0.1} };
higedura	0:f5a5a0e5fefb	255	double Q[N][N] = { {0.00263, 0, 0}, {0, 0.00373, 0}, {0, 0, 0.00509} };
higedura	0:f5a5a0e5fefb	256
higedura	0:f5a5a0e5fefb	257	double Fjac[N][N] = {{x[1]cos(y[0])tan(y[1])-x[2]sin(y[0])tan(y[1]), x[1]sin(y[0])/(cos(y[1])cos(y[1]))+x[2]cos(y[0])/(cos(y[1])cos(y[1])), 0}, {-x[1]sin(y[0])-x[2]cos(y[0]), 0, 0}, {x[1]cos(y[0])/cos(y[1])-x[2]sin(y[0])/cos(y[1]), x[1]sin(y[0])sin(y[1])/(cos(y[1])cos(y[1]))+x[2]cos(y[0])sin(y[1])/(cos(y[1])cos(y[1])), 0}};
higedura	0:f5a5a0e5fefb	258	double Fjac_t[N][N] = {{x[1]cos(y[0])tan(y[1])-x[2]sin(y[0])tan(y[1]), -x[1]sin(y[0])-x[2]cos(y[0]), x[1]cos(y[0])/cos(y[1])-x[2]sin(y[0])/cos(y[1])}, {x[1]sin(y[0])/(cos(y[1])cos(y[1]))+x[2]cos(y[0])/(cos(y[1])cos(y[1])), 0, x[1]sin(y[0])sin(y[1])/(cos(y[1])cos(y[1]))+x[2]cos(y[0])sin(y[1])/(cos(y[1])cos(y[1]))}, {0, 0, 0}};
higedura	0:f5a5a0e5fefb	259	double Gjac[N][N] = {{1, sin(y[0])tan(y[1]), cos(y[0])tan(y[1])}, {0, cos(y[0]), -sin(y[0])}, {0, sin(y[0])/cos(y[1]), cos(y[0])/cos(y[1])}};
higedura	0:f5a5a0e5fefb	260	double Gjac_t[N][N] = {{1, 0, 0}, {sin(y[0])tan(y[1]), cos(y[0]), sin(y[0])/cos(y[1])}, {cos(y[0])tan(y[1]), -sin(y[0]), cos(y[0])/cos(y[1])}};
higedura	0:f5a5a0e5fefb	261
higedura	0:f5a5a0e5fefb	262	double FPF[N][N] = {0};
higedura	0:f5a5a0e5fefb	263	double GQG[N][N] = {0};
higedura	0:f5a5a0e5fefb	264
higedura	0:f5a5a0e5fefb	265	double FP[N][N] = {0};
higedura	0:f5a5a0e5fefb	266	double GQ[N][N] = {0};
higedura	0:f5a5a0e5fefb	267
higedura	0:f5a5a0e5fefb	268	for( int i=0;i<N;i++ ){
higedura	0:f5a5a0e5fefb	269	for( int j=0;j<N;j++ ){
higedura	0:f5a5a0e5fefb	270	for( int k=0;k<N;k++ ){
higedura	0:f5a5a0e5fefb	271	FP[i][j] += Fjac[i][k]*P[k][j];
higedura	0:f5a5a0e5fefb	272	GQ[i][j] += Gjac[i][k]*Q[k][j];
higedura	0:f5a5a0e5fefb	273
higedura	0:f5a5a0e5fefb	274	}
higedura	0:f5a5a0e5fefb	275	}
higedura	0:f5a5a0e5fefb	276	}
higedura	0:f5a5a0e5fefb	277
higedura	0:f5a5a0e5fefb	278	for( int i=0;i<N;i++ ){
higedura	0:f5a5a0e5fefb	279	for( int j=0;j<N;j++ ){
higedura	0:f5a5a0e5fefb	280	for( int k=0;k<N;k++ ){
higedura	0:f5a5a0e5fefb	281	FPF[i][j] += FP[i][k]*Fjac_t[k][j];
higedura	0:f5a5a0e5fefb	282	GQG[i][j] += GQ[i][k]*Gjac_t[k][j];
higedura	0:f5a5a0e5fefb	283	}
higedura	0:f5a5a0e5fefb	284	}
higedura	0:f5a5a0e5fefb	285	}
higedura	0:f5a5a0e5fefb	286	for( int i=0;i<N;i++ ){
higedura	0:f5a5a0e5fefb	287	for( int j=0;j<N;j++ ){
higedura	0:f5a5a0e5fefb	288	P[i][j] = FPF[i][j]+GQG[i][j];
higedura	0:f5a5a0e5fefb	289	}
higedura	0:f5a5a0e5fefb	290	}
higedura	0:f5a5a0e5fefb	291
higedura	0:f5a5a0e5fefb	292	return 0;
higedura	0:f5a5a0e5fefb	293
higedura	0:f5a5a0e5fefb	294	}
higedura	0:f5a5a0e5fefb	295
higedura	0:f5a5a0e5fefb	296	double* EKF_correct( double y[N], double x[N], double z[N] ){
higedura	0:f5a5a0e5fefb	297	// K = P * H' / ( H * P * H' + R )
higedura	0:f5a5a0e5fefb	298	// x = x + K * ( yobs(t) - H * x )
higedura	0:f5a5a0e5fefb	299	// P = P - K * H * P
higedura	0:f5a5a0e5fefb	300
higedura	0:f5a5a0e5fefb	301	//double R[N][N] = { {0.15, 0, 0}, {0, 0.15, 0}, {0, 0, 0.15} };
higedura	0:f5a5a0e5fefb	302	double R[N][N] = { {0.00015, 0, 0}, {0, 0.00016, 0}, {0, 0, 0.00032} };
higedura	0:f5a5a0e5fefb	303
higedura	0:f5a5a0e5fefb	304	double Hjac[N][N] = {{0, cos(y[1]), 0}, {cos(y[0]), 0, 0}, {-sin(y[0])cos(y[1]), -cos(y[0])sin(y[1]), 0}};
higedura	0:f5a5a0e5fefb	305	double Hjac_t[N][N] = {{0, cos(y[0]), -sin(y[0])cos(y[1])}, {cos(y[1]), 0, -cos(y[0])sin(y[1])}, {0, 0, 0}};
higedura	0:f5a5a0e5fefb	306	double K[N][N] = {0}; // Kalman gain
higedura	0:f5a5a0e5fefb	307	double K_deno[N][N] = {0}; // Denominator of the kalman gain
higedura	0:f5a5a0e5fefb	308	double det_K_deno_inv = 0;
higedura	0:f5a5a0e5fefb	309	double K_deno_inv[N][N] = {0};
higedura	0:f5a5a0e5fefb	310	double HPH[N][N] = {0};
higedura	0:f5a5a0e5fefb	311	double HP[N][N] = {0};
higedura	0:f5a5a0e5fefb	312	double PH[N][N] = {0};
higedura	0:f5a5a0e5fefb	313	double KHP[N][N] = {0};
higedura	0:f5a5a0e5fefb	314
higedura	0:f5a5a0e5fefb	315	double Hx[N] = {0};
higedura	0:f5a5a0e5fefb	316	double z_Hx[N] = {0};
higedura	0:f5a5a0e5fefb	317	double Kz_Hx[N] = {0};
higedura	0:f5a5a0e5fefb	318
higedura	0:f5a5a0e5fefb	319	double* py = y;
higedura	0:f5a5a0e5fefb	320
higedura	0:f5a5a0e5fefb	321	// Kalman gain
higedura	0:f5a5a0e5fefb	322	for( int i=0;i<N;i++ ){
higedura	0:f5a5a0e5fefb	323	for( int j=0;j<N;j++ ){
higedura	0:f5a5a0e5fefb	324	for( int k=0;k<N;k++ ){
higedura	0:f5a5a0e5fefb	325	HP[i][j] += Hjac[i][k]*P[k][j];
higedura	0:f5a5a0e5fefb	326	PH[i][j] += P[i][k]*Hjac_t[k][j];
higedura	0:f5a5a0e5fefb	327	}
higedura	0:f5a5a0e5fefb	328	}
higedura	0:f5a5a0e5fefb	329	}
higedura	0:f5a5a0e5fefb	330	for( int i=0;i<N;i++ ){
higedura	0:f5a5a0e5fefb	331	for( int j=0;j<N;j++ ){
higedura	0:f5a5a0e5fefb	332	for( int k=0;k<N;k++ ){
higedura	0:f5a5a0e5fefb	333	HPH[i][j] += HP[i][k]*Hjac_t[k][j];
higedura	0:f5a5a0e5fefb	334	}
higedura	0:f5a5a0e5fefb	335	}
higedura	0:f5a5a0e5fefb	336	}
higedura	0:f5a5a0e5fefb	337	for( int i=0;i<N;i++ ){
higedura	0:f5a5a0e5fefb	338	for( int j=0;j<N;j++ ){
higedura	0:f5a5a0e5fefb	339	K_deno[i][j] = HPH[i][j]+R[i][j];
higedura	0:f5a5a0e5fefb	340	}
higedura	0:f5a5a0e5fefb	341	}
higedura	0:f5a5a0e5fefb	342	// Inverce matrix
higedura	0:f5a5a0e5fefb	343	det_K_deno_inv = K_deno[0][0]K_deno[1][1]K_deno[2][2]+K_deno[1][0]K_deno[2][1]K_deno[0][2]+K_deno[2][0]K_deno[0][1]K_deno[1][2]-K_deno[0][0]K_deno[2][1]K_deno[1][2]-K_deno[2][0]K_deno[1][1]K_deno[0][2]-K_deno[1][0]K_deno[0][1]K_deno[2][2];
higedura	0:f5a5a0e5fefb	344	K_deno_inv[0][0] = (K_deno[1][1]K_deno[2][2]-K_deno[1][2]K_deno[2][1])/det_K_deno_inv;
higedura	0:f5a5a0e5fefb	345	K_deno_inv[0][1] = (K_deno[0][2]K_deno[2][1]-K_deno[0][1]K_deno[2][2])/det_K_deno_inv;
higedura	0:f5a5a0e5fefb	346	K_deno_inv[0][2] = (K_deno[0][1]K_deno[1][2]-K_deno[0][2]K_deno[1][1])/det_K_deno_inv;
higedura	0:f5a5a0e5fefb	347	K_deno_inv[1][0] = (K_deno[1][2]K_deno[2][0]-K_deno[1][0]K_deno[2][2])/det_K_deno_inv;
higedura	0:f5a5a0e5fefb	348	K_deno_inv[1][1] = (K_deno[0][0]K_deno[2][2]-K_deno[0][2]K_deno[2][0])/det_K_deno_inv;
higedura	0:f5a5a0e5fefb	349	K_deno_inv[1][2] = (K_deno[0][2]K_deno[1][0]-K_deno[0][0]K_deno[1][2])/det_K_deno_inv;
higedura	0:f5a5a0e5fefb	350	K_deno_inv[2][0] = (K_deno[1][0]K_deno[2][1]-K_deno[1][1]K_deno[2][0])/det_K_deno_inv;
higedura	0:f5a5a0e5fefb	351	K_deno_inv[2][1] = (K_deno[0][1]K_deno[2][0]-K_deno[0][0]K_deno[2][1])/det_K_deno_inv;
higedura	0:f5a5a0e5fefb	352	K_deno_inv[2][2] = (K_deno[0][0]K_deno[1][1]-K_deno[0][1]K_deno[1][0])/det_K_deno_inv;
higedura	0:f5a5a0e5fefb	353
higedura	0:f5a5a0e5fefb	354	for( int i=0;i<N;i++ ){
higedura	0:f5a5a0e5fefb	355	for( int j=0;j<N;j++ ){
higedura	0:f5a5a0e5fefb	356	for( int k=0;k<N;k++ ){
higedura	0:f5a5a0e5fefb	357	K[i][j] += PH[i][k]*K_deno_inv[k][j];
higedura	0:f5a5a0e5fefb	358	}
higedura	0:f5a5a0e5fefb	359	}
higedura	0:f5a5a0e5fefb	360	}
higedura	0:f5a5a0e5fefb	361
higedura	0:f5a5a0e5fefb	362	// Filtering
higedura	0:f5a5a0e5fefb	363	for( int i=0;i<N;i++ ){
higedura	0:f5a5a0e5fefb	364	for( int j=0;j<N;j++ ){
higedura	0:f5a5a0e5fefb	365	Hx[i] += Hjac[i][j]*y[j];
higedura	0:f5a5a0e5fefb	366	}
higedura	0:f5a5a0e5fefb	367	}
higedura	0:f5a5a0e5fefb	368	for( int i=0;i<N;i++ ){
higedura	0:f5a5a0e5fefb	369	z_Hx[i] = z[i]-Hx[i];
higedura	0:f5a5a0e5fefb	370	}
higedura	0:f5a5a0e5fefb	371	for( int i=0;i<N;i++ ){
higedura	0:f5a5a0e5fefb	372	for( int j=0;j<N;j++ ){
higedura	0:f5a5a0e5fefb	373	Kz_Hx[i] += K[i][j]*z_Hx[j];
higedura	0:f5a5a0e5fefb	374	}
higedura	0:f5a5a0e5fefb	375	}
higedura	0:f5a5a0e5fefb	376	for( int i=0;i<N;i++ ){
higedura	0:f5a5a0e5fefb	377	y[i] = y[i]+Kz_Hx[i];
higedura	0:f5a5a0e5fefb	378	}
higedura	0:f5a5a0e5fefb	379
higedura	0:f5a5a0e5fefb	380	// Covarience
higedura	0:f5a5a0e5fefb	381	for( int i=0;i<N;i++ ){
higedura	0:f5a5a0e5fefb	382	for( int j=0;j<N;j++ ){
higedura	0:f5a5a0e5fefb	383	for( int k=0;k<N;k++ ){
higedura	0:f5a5a0e5fefb	384	KHP[i][j] += K[i][k]*HP[k][j];
higedura	0:f5a5a0e5fefb	385	}
higedura	0:f5a5a0e5fefb	386	}
higedura	0:f5a5a0e5fefb	387	}
higedura	0:f5a5a0e5fefb	388	for( int i=0;i<N;i++ ){
higedura	0:f5a5a0e5fefb	389	for( int j=0;j<N;j++ ){
higedura	0:f5a5a0e5fefb	390	P[i][j] = P[i][j]-KHP[i][j];
higedura	0:f5a5a0e5fefb	391	}
higedura	0:f5a5a0e5fefb	392	}
higedura	0:f5a5a0e5fefb	393
higedura	0:f5a5a0e5fefb	394	return py;
higedura	0:f5a5a0e5fefb	395
higedura	0:f5a5a0e5fefb	396	}
higedura	0:f5a5a0e5fefb	397
higedura	0:f5a5a0e5fefb	398	double* LWMA( double z[N] ){
higedura	0:f5a5a0e5fefb	399
higedura	0:f5a5a0e5fefb	400	double zLWMA[N] = {0};
higedura	0:f5a5a0e5fefb	401	double zLWMA_num[N] = {0};
higedura	0:f5a5a0e5fefb	402	double zLWMA_den = 0;
higedura	0:f5a5a0e5fefb	403
higedura	0:f5a5a0e5fefb	404	double* pzLWMA = zLWMA;
higedura	0:f5a5a0e5fefb	405
higedura	0:f5a5a0e5fefb	406	for( int i=1;i<N_LWMA;i++ ){
higedura	0:f5a5a0e5fefb	407	for( int j=0;j<N;j++ ){
higedura	0:f5a5a0e5fefb	408	z_buf[j][N_LWMA-i] = z_buf[j][N_LWMA-i-1];
higedura	0:f5a5a0e5fefb	409	}
higedura	0:f5a5a0e5fefb	410	}
higedura	0:f5a5a0e5fefb	411	for( int i=0;i<N;i++ ){
higedura	0:f5a5a0e5fefb	412	z_buf[i][0] = z[i];
higedura	0:f5a5a0e5fefb	413	}
higedura	0:f5a5a0e5fefb	414
higedura	0:f5a5a0e5fefb	415	for( int i=0;i<N_LWMA;i++ ){
higedura	0:f5a5a0e5fefb	416	for( int j=0;j<N;j++ ){
higedura	0:f5a5a0e5fefb	417	zLWMA_num[j] += (N_LWMA-i)*z_buf[j][i];
higedura	0:f5a5a0e5fefb	418	}
higedura	0:f5a5a0e5fefb	419	zLWMA_den += N_LWMA-i;
higedura	0:f5a5a0e5fefb	420	}
higedura	0:f5a5a0e5fefb	421	for( int i=0;i<N;i++ ){
higedura	0:f5a5a0e5fefb	422	zLWMA[i] = zLWMA_num[i]/zLWMA_den;
higedura	0:f5a5a0e5fefb	423	}
higedura	0:f5a5a0e5fefb	424
higedura	0:f5a5a0e5fefb	425	return pzLWMA;
higedura	0:f5a5a0e5fefb	426
higedura	0:f5a5a0e5fefb	427	}
higedura	0:f5a5a0e5fefb	428
higedura	0:f5a5a0e5fefb	429	double* RK4( double dt, double y[N], double x[N] ){
higedura	0:f5a5a0e5fefb	430
higedura	0:f5a5a0e5fefb	431	double yBuf[N] = {0};
higedura	0:f5a5a0e5fefb	432	double k[N][4] = {0};
higedura	0:f5a5a0e5fefb	433
higedura	0:f5a5a0e5fefb	434	double* p_y = y;
higedura	0:f5a5a0e5fefb	435
higedura	0:f5a5a0e5fefb	436	double* pk1;
higedura	0:f5a5a0e5fefb	437	double* pk2;
higedura	0:f5a5a0e5fefb	438	double* pk3;
higedura	0:f5a5a0e5fefb	439	double* pk4;
higedura	0:f5a5a0e5fefb	440
higedura	0:f5a5a0e5fefb	441	for( int i=0;i<N;i++){ yBuf[i] = y[i]; }
higedura	0:f5a5a0e5fefb	442	pk1 = func (yBuf,x);
higedura	0:f5a5a0e5fefb	443	for( int i=0;i<N;i++ ){ k[i][0] = *pk1; pk1 = pk1+1; }
higedura	0:f5a5a0e5fefb	444
higedura	0:f5a5a0e5fefb	445	for( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5dtk[i][1]; }
higedura	0:f5a5a0e5fefb	446	pk2 = func (yBuf,x);
higedura	0:f5a5a0e5fefb	447	for( int i=0;i<N;i++ ){ k[i][1] = *pk2; pk2 = pk2+1; }
higedura	0:f5a5a0e5fefb	448
higedura	0:f5a5a0e5fefb	449	for( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5dtk[i][2]; }
higedura	0:f5a5a0e5fefb	450	pk3 = func (yBuf,x);
higedura	0:f5a5a0e5fefb	451	for( int i=0;i<N;i++ ){ k[i][2] = *pk3; pk3 = pk3+1; }
higedura	0:f5a5a0e5fefb	452
higedura	0:f5a5a0e5fefb	453	for( int i=0;i<N;i++){ yBuf[i] = y[i]+dt*k[i][3]; }
higedura	0:f5a5a0e5fefb	454	pk4 = func (yBuf,x);
higedura	0:f5a5a0e5fefb	455	for( int i=0;i<N;i++ ){ k[i][3] = *pk4; pk4 = pk4+1; }
higedura	0:f5a5a0e5fefb	456
higedura	0:f5a5a0e5fefb	457	for( int i=0;i<N;i++){ y[i] = y[i]+dt(k[i][0]+2.0k[i][1]+2.0*k[i][2]+k[i][3])/6.0; }
higedura	0:f5a5a0e5fefb	458
higedura	0:f5a5a0e5fefb	459	return p_y;
higedura	0:f5a5a0e5fefb	460
higedura	0:f5a5a0e5fefb	461	}
higedura	0:f5a5a0e5fefb	462
higedura	0:f5a5a0e5fefb	463	double* func( double y[N], double x[N] ){
higedura	0:f5a5a0e5fefb	464
higedura	0:f5a5a0e5fefb	465	double f[N] = {0};
higedura	0:f5a5a0e5fefb	466	double* p_f = f;
higedura	0:f5a5a0e5fefb	467
higedura	0:f5a5a0e5fefb	468	f[0] = x[0]+x[1]sin(y[0])tan(y[1])+x[2]cos(y[0])tan(y[1]);
higedura	0:f5a5a0e5fefb	469	f[1] = x[1]cos(y[0])-x[2]sin(y[0]);
higedura	0:f5a5a0e5fefb	470	f[2] = x[1]sin(y[0])/cos(y[1])+x[2]cos(y[0])/cos(y[1]);
higedura	0:f5a5a0e5fefb	471
higedura	0:f5a5a0e5fefb	472	return p_f;
higedura	0:f5a5a0e5fefb	473
higedura	0:f5a5a0e5fefb	474	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	23 Apr 2012
Imports:	32
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	2
Followers:	2

main.cpp@0:f5a5a0e5fefb, 2012-04-23 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning