Eurobot_2012_Secondary - Eurobot_2012_Secondary

Users » narshu » Code » Eurobot_2012_Secondary

Shuto Naruse / Mbed 2 deprecated Eurobot_2012_Secondary

Eurobot_2012_Secondary

Kalman/Kalman.cpp@0:fbfafa6bf5f9, 2012-04-20 (annotated)

Committer:: narshu
Date:: Fri Apr 20 21:32:24 2012 +0000
Revision:: 0:fbfafa6bf5f9

Who changed what in which revision?

User	Revision	Line number	New contents of line
narshu	0:fbfafa6bf5f9	1	//***************************************************************************************
narshu	0:fbfafa6bf5f9	2	//Kalman Filter implementation
narshu	0:fbfafa6bf5f9	3	//***************************************************************************************
narshu	0:fbfafa6bf5f9	4	#include "Kalman.h"
narshu	0:fbfafa6bf5f9	5	#include "rtos.h"
narshu	0:fbfafa6bf5f9	6	#include "RFSRF05.h"
narshu	0:fbfafa6bf5f9	7	//#include "MatrixMath.h"
narshu	0:fbfafa6bf5f9	8	//#include "Matrix.h"
narshu	0:fbfafa6bf5f9	9	#include "math.h"
narshu	0:fbfafa6bf5f9	10	#include "globals.h"
narshu	0:fbfafa6bf5f9	11	#include "motors.h"
narshu	0:fbfafa6bf5f9	12	#include "system.h"
narshu	0:fbfafa6bf5f9	13	#include "geometryfuncs.h"
narshu	0:fbfafa6bf5f9	14
narshu	0:fbfafa6bf5f9	15	#include <tvmet/Matrix.h>
narshu	0:fbfafa6bf5f9	16	#include <tvmet/Vector.h>
narshu	0:fbfafa6bf5f9	17	using namespace tvmet;
narshu	0:fbfafa6bf5f9	18	DigitalOut led1(LED1);
narshu	0:fbfafa6bf5f9	19	DigitalOut led2(LED2);
narshu	0:fbfafa6bf5f9	20	DigitalOut led3(LED3);
narshu	0:fbfafa6bf5f9	21	DigitalOut led4(LED4);
narshu	0:fbfafa6bf5f9	22
narshu	0:fbfafa6bf5f9	23
narshu	0:fbfafa6bf5f9	24	Kalman::Kalman(Motors &motorsin) :
narshu	0:fbfafa6bf5f9	25	sonararray(p10,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9),
narshu	0:fbfafa6bf5f9	26	motors(motorsin),
narshu	0:fbfafa6bf5f9	27	predictthread(predictloopwrapper, this, osPriorityNormal, 512),
narshu	0:fbfafa6bf5f9	28	predictticker( SIGTICKARGS(predictthread, 0x1) ),
narshu	0:fbfafa6bf5f9	29	// sonarthread(sonarloopwrapper, this, osPriorityNormal, 256),
narshu	0:fbfafa6bf5f9	30	// sonarticker( SIGTICKARGS(sonarthread, 0x1) ),
narshu	0:fbfafa6bf5f9	31	updatethread(updateloopwrapper, this, osPriorityNormal, 2048) {
narshu	0:fbfafa6bf5f9	32
narshu	0:fbfafa6bf5f9	33	//Initilising matrices
narshu	0:fbfafa6bf5f9	34
narshu	0:fbfafa6bf5f9	35	// X = x, y, theta;
narshu	0:fbfafa6bf5f9	36	X = 0.5, 0, 0;
narshu	0:fbfafa6bf5f9	37
narshu	0:fbfafa6bf5f9	38	P = 1, 0, 0,
narshu	0:fbfafa6bf5f9	39	0, 1, 0,
narshu	0:fbfafa6bf5f9	40	0, 0, 0.04;
narshu	0:fbfafa6bf5f9	41
narshu	0:fbfafa6bf5f9	42	//Q = 0.002, 0, 0, //temporary matrix, Use dt!
narshu	0:fbfafa6bf5f9	43	// 0, 0.002, 0,
narshu	0:fbfafa6bf5f9	44	// 0, 0, 0.002;
narshu	0:fbfafa6bf5f9	45
narshu	0:fbfafa6bf5f9	46	//measurment variance R is provided by each sensor when calling runupdate
narshu	0:fbfafa6bf5f9	47
narshu	0:fbfafa6bf5f9	48	//attach callback
narshu	0:fbfafa6bf5f9	49	sonararray.callbackobj = (DummyCT*)this;
narshu	0:fbfafa6bf5f9	50	sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance, float variance)) &Kalman::runupdate;
narshu	0:fbfafa6bf5f9	51
narshu	0:fbfafa6bf5f9	52
narshu	0:fbfafa6bf5f9	53	predictticker.start(20);
narshu	0:fbfafa6bf5f9	54	// sonarticker.start(50);
narshu	0:fbfafa6bf5f9	55
narshu	0:fbfafa6bf5f9	56
narshu	0:fbfafa6bf5f9	57	}
narshu	0:fbfafa6bf5f9	58
narshu	0:fbfafa6bf5f9	59
narshu	0:fbfafa6bf5f9	60	void Kalman::predictloop() {
narshu	0:fbfafa6bf5f9	61
narshu	0:fbfafa6bf5f9	62	float lastleft = 0;
narshu	0:fbfafa6bf5f9	63	float lastright = 0;
narshu	0:fbfafa6bf5f9	64
narshu	0:fbfafa6bf5f9	65	while (1) {
narshu	0:fbfafa6bf5f9	66	Thread::signal_wait(0x1);
narshu	0:fbfafa6bf5f9	67	led1 = !led1;
narshu	0:fbfafa6bf5f9	68
narshu	0:fbfafa6bf5f9	69	int leftenc = motors.getEncoder1();
narshu	0:fbfafa6bf5f9	70	int rightenc = motors.getEncoder2();
narshu	0:fbfafa6bf5f9	71
narshu	0:fbfafa6bf5f9	72	float dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f;
narshu	0:fbfafa6bf5f9	73	float dright = motors.encoderToDistance(rightenc-lastright)/1000.0f;
narshu	0:fbfafa6bf5f9	74
narshu	0:fbfafa6bf5f9	75	lastleft = leftenc;
narshu	0:fbfafa6bf5f9	76	lastright = rightenc;
narshu	0:fbfafa6bf5f9	77
narshu	0:fbfafa6bf5f9	78
narshu	0:fbfafa6bf5f9	79	//The below calculation are in body frame (where +x is forward)
narshu	0:fbfafa6bf5f9	80	float dxp, dyp,d,r;
narshu	0:fbfafa6bf5f9	81	float thetap = (dright - dleft)*PI / (float(robotCircumference)/1000.0f);
narshu	0:fbfafa6bf5f9	82	if (abs(thetap) < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
narshu	0:fbfafa6bf5f9	83	d = (dright + dleft)/2.0f;
narshu	0:fbfafa6bf5f9	84	dxp = d*cos(thetap/2.0f);
narshu	0:fbfafa6bf5f9	85	dyp = d*sin(thetap/2.0f);
narshu	0:fbfafa6bf5f9	86
narshu	0:fbfafa6bf5f9	87	} else { //calculate circle arc
narshu	0:fbfafa6bf5f9	88	//float r = (right + left) / (4.0f * PI * thetap);
narshu	0:fbfafa6bf5f9	89	r = (dright + dleft) / (2.0f*thetap);
narshu	0:fbfafa6bf5f9	90	dxp = abs(r)*sin(thetap);
narshu	0:fbfafa6bf5f9	91	dyp = r - r*cos(thetap);
narshu	0:fbfafa6bf5f9	92	}
narshu	0:fbfafa6bf5f9	93
narshu	0:fbfafa6bf5f9	94	statelock.lock();
narshu	0:fbfafa6bf5f9	95
narshu	0:fbfafa6bf5f9	96	//rotating to cartesian frame and updating state
narshu	0:fbfafa6bf5f9	97	X(0) += dxp * cos(X(2)) - dyp * sin(X(2));
narshu	0:fbfafa6bf5f9	98	X(1) += dxp * sin(X(2)) + dyp * cos(X(2));
narshu	0:fbfafa6bf5f9	99	X(2) = rectifyAng(X(2) + thetap);
narshu	0:fbfafa6bf5f9	100
narshu	0:fbfafa6bf5f9	101	//Linearising F around X
narshu	0:fbfafa6bf5f9	102	Matrix<float, 3, 3> F;
narshu	0:fbfafa6bf5f9	103	F = 1, 0, (dxp * -sin(X(2)) - dyp * cos(X(2))),
narshu	0:fbfafa6bf5f9	104	0, 1, (dxp * cos(X(2)) - dyp * sin(X(2))),
narshu	0:fbfafa6bf5f9	105	0, 0, 1;
narshu	0:fbfafa6bf5f9	106
narshu	0:fbfafa6bf5f9	107	//Generating forward and rotational variance
narshu	0:fbfafa6bf5f9	108	float varfwd = fwdvarperunit * (dright + dleft) / 2.0f;
narshu	0:fbfafa6bf5f9	109	float varang = varperang * thetap;
narshu	0:fbfafa6bf5f9	110	float varxydt = xyvarpertime * PREDICTPERIOD;
narshu	0:fbfafa6bf5f9	111	float varangdt = angvarpertime * PREDICTPERIOD;
narshu	0:fbfafa6bf5f9	112
narshu	0:fbfafa6bf5f9	113	//Rotating into cartesian frame
narshu	0:fbfafa6bf5f9	114	Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
narshu	0:fbfafa6bf5f9	115	Qsub = varfwd + varxydt, 0,
narshu	0:fbfafa6bf5f9	116	0, varxydt;
narshu	0:fbfafa6bf5f9	117
narshu	0:fbfafa6bf5f9	118	Qrot = Rotmatrix(X(2));
narshu	0:fbfafa6bf5f9	119
narshu	0:fbfafa6bf5f9	120	Qsubrot = Qrot * Qsub * trans(Qrot);
narshu	0:fbfafa6bf5f9	121
narshu	0:fbfafa6bf5f9	122	//Generate Q
narshu	0:fbfafa6bf5f9	123	Matrix<float, 3, 3> Q;//(Qsubrot);
narshu	0:fbfafa6bf5f9	124	Q = Qsubrot(0,0), Qsubrot(0,1), 0,
narshu	0:fbfafa6bf5f9	125	Qsubrot(1,0), Qsubrot(1,1), 0,
narshu	0:fbfafa6bf5f9	126	0, 0, varang + varangdt;
narshu	0:fbfafa6bf5f9	127
narshu	0:fbfafa6bf5f9	128	P = F * P * trans(F) + Q;
narshu	0:fbfafa6bf5f9	129
narshu	0:fbfafa6bf5f9	130	statelock.unlock();
narshu	0:fbfafa6bf5f9	131	//Thread::wait(PREDICTPERIOD);
narshu	0:fbfafa6bf5f9	132
narshu	0:fbfafa6bf5f9	133	//cout << "predict" << X << endl;
narshu	0:fbfafa6bf5f9	134	//cout << P << endl;
narshu	0:fbfafa6bf5f9	135	}
narshu	0:fbfafa6bf5f9	136	}
narshu	0:fbfafa6bf5f9	137
narshu	0:fbfafa6bf5f9	138	//void Kalman::sonarloop() {
narshu	0:fbfafa6bf5f9	139	// while (1) {
narshu	0:fbfafa6bf5f9	140	// Thread::signal_wait(0x1);
narshu	0:fbfafa6bf5f9	141	// sonararray.startRange();
narshu	0:fbfafa6bf5f9	142	// }
narshu	0:fbfafa6bf5f9	143	//}
narshu	0:fbfafa6bf5f9	144
narshu	0:fbfafa6bf5f9	145
narshu	0:fbfafa6bf5f9	146	void Kalman::runupdate(measurement_t type, float value, float variance) {
narshu	0:fbfafa6bf5f9	147	//printf("beacon %d dist %f\r\n", sonarid, dist);
narshu	0:fbfafa6bf5f9	148	//led2 = !led2;
narshu	0:fbfafa6bf5f9	149
narshu	0:fbfafa6bf5f9	150	measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
narshu	0:fbfafa6bf5f9	151	if (measured) {
narshu	0:fbfafa6bf5f9	152	measured->mtype = type;
narshu	0:fbfafa6bf5f9	153	measured->value = value;
narshu	0:fbfafa6bf5f9	154	measured->variance = variance;
narshu	0:fbfafa6bf5f9	155
narshu	0:fbfafa6bf5f9	156	osStatus putret = measureMQ.put(measured);
narshu	0:fbfafa6bf5f9	157	if (putret)
narshu	0:fbfafa6bf5f9	158	led4 = 1;
narshu	0:fbfafa6bf5f9	159	// printf("putting in MQ error code %#x\r\n", putret);
narshu	0:fbfafa6bf5f9	160	} else {
narshu	0:fbfafa6bf5f9	161	led4 = 1;
narshu	0:fbfafa6bf5f9	162	//printf("MQalloc returned NULL ptr\r\n");
narshu	0:fbfafa6bf5f9	163	}
narshu	0:fbfafa6bf5f9	164
narshu	0:fbfafa6bf5f9	165	}
narshu	0:fbfafa6bf5f9	166
narshu	0:fbfafa6bf5f9	167	void Kalman::updateloop() {
narshu	0:fbfafa6bf5f9	168	measurement_t type;
narshu	0:fbfafa6bf5f9	169	float value,variance,rbx,rby,expecdist,Y;
narshu	0:fbfafa6bf5f9	170	float dhdx,dhdy;
narshu	0:fbfafa6bf5f9	171	bool aborton2stddev = false;
narshu	0:fbfafa6bf5f9	172
narshu	0:fbfafa6bf5f9	173	Matrix<float, 1, 3> H;
narshu	0:fbfafa6bf5f9	174
narshu	0:fbfafa6bf5f9	175	float S;
narshu	0:fbfafa6bf5f9	176	Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() );
narshu	0:fbfafa6bf5f9	177
narshu	0:fbfafa6bf5f9	178
narshu	0:fbfafa6bf5f9	179	while (1) {
narshu	0:fbfafa6bf5f9	180	led2 = !led2;
narshu	0:fbfafa6bf5f9	181
narshu	0:fbfafa6bf5f9	182	osEvent evt = measureMQ.get();
narshu	0:fbfafa6bf5f9	183
narshu	0:fbfafa6bf5f9	184	if (evt.status == osEventMail) {
narshu	0:fbfafa6bf5f9	185
narshu	0:fbfafa6bf5f9	186	measurmentdata &measured = (measurmentdata)evt.value.p;
narshu	0:fbfafa6bf5f9	187	type = measured.mtype; //Note, may support more measurment types than sonar in the future!
narshu	0:fbfafa6bf5f9	188	value = measured.value;
narshu	0:fbfafa6bf5f9	189	variance = measured.variance;
narshu	0:fbfafa6bf5f9	190
narshu	0:fbfafa6bf5f9	191	// don't forget to free the memory
narshu	0:fbfafa6bf5f9	192	measureMQ.free(&measured);
narshu	0:fbfafa6bf5f9	193
narshu	0:fbfafa6bf5f9	194	if (type <= maxmeasure) {
narshu	0:fbfafa6bf5f9	195
narshu	0:fbfafa6bf5f9	196	if (type <= SONAR3) {
narshu	0:fbfafa6bf5f9	197
narshu	0:fbfafa6bf5f9	198	float dist = value / 1000.0f; //converting to m from mm
narshu	0:fbfafa6bf5f9	199	int sonarid = type;
narshu	0:fbfafa6bf5f9	200	aborton2stddev = false;
narshu	0:fbfafa6bf5f9	201
narshu	0:fbfafa6bf5f9	202	statelock.lock();
narshu	0:fbfafa6bf5f9	203	SonarMeasures[sonarid] = dist; //update the current sonar readings
narshu	0:fbfafa6bf5f9	204
narshu	0:fbfafa6bf5f9	205	rbx = X(0) - beaconpos[sonarid].x/1000.0f;
narshu	0:fbfafa6bf5f9	206	rby = X(1) - beaconpos[sonarid].y/1000.0f;
narshu	0:fbfafa6bf5f9	207
narshu	0:fbfafa6bf5f9	208	expecdist = hypot(rbx, rby);//sqrt(rbxrbx + rbyrby);
narshu	0:fbfafa6bf5f9	209	Y = dist - expecdist;
narshu	0:fbfafa6bf5f9	210
narshu	0:fbfafa6bf5f9	211	dhdx = rbx / expecdist;
narshu	0:fbfafa6bf5f9	212	dhdy = rby / expecdist;
narshu	0:fbfafa6bf5f9	213
narshu	0:fbfafa6bf5f9	214	H = dhdx, dhdy, 0;
narshu	0:fbfafa6bf5f9	215
narshu	0:fbfafa6bf5f9	216	} else if (type <= IR3) {
narshu	0:fbfafa6bf5f9	217
narshu	0:fbfafa6bf5f9	218	aborton2stddev = false;
narshu	0:fbfafa6bf5f9	219	int IRidx = type-3;
narshu	0:fbfafa6bf5f9	220
narshu	0:fbfafa6bf5f9	221	statelock.lock();
narshu	0:fbfafa6bf5f9	222	IRMeasures[IRidx] = value;
narshu	0:fbfafa6bf5f9	223
narshu	0:fbfafa6bf5f9	224	rbx = X(0) - beaconpos[IRidx].x/1000.0f;
narshu	0:fbfafa6bf5f9	225	rby = X(1) - beaconpos[IRidx].y/1000.0f;
narshu	0:fbfafa6bf5f9	226
narshu	0:fbfafa6bf5f9	227	float expecang = atan2(-rbx, -rby) - X(2);
narshu	0:fbfafa6bf5f9	228	//printf("expecang: %0.4f, value: %0.4f \n\r", expecang180/PI,value180/PI);
narshu	0:fbfafa6bf5f9	229	Y = rectifyAng(value + expecang);
narshu	0:fbfafa6bf5f9	230
narshu	0:fbfafa6bf5f9	231	float dstsq = rbxrbx + rbyrby;
narshu	0:fbfafa6bf5f9	232	H = -rby/dstsq, rbx/dstsq, -1;
narshu	0:fbfafa6bf5f9	233	}
narshu	0:fbfafa6bf5f9	234
narshu	0:fbfafa6bf5f9	235	Matrix<float, 3, 1> PH (P * trans(H));
narshu	0:fbfafa6bf5f9	236	S = (H * PH)(0,0) + variance;
narshu	0:fbfafa6bf5f9	237
narshu	0:fbfafa6bf5f9	238	if (aborton2stddev && YY > 4 S) {
narshu	0:fbfafa6bf5f9	239	statelock.unlock();
narshu	0:fbfafa6bf5f9	240	continue;
narshu	0:fbfafa6bf5f9	241	}
narshu	0:fbfafa6bf5f9	242
narshu	0:fbfafa6bf5f9	243	Matrix<float, 3, 1> K (PH * (1/S));
narshu	0:fbfafa6bf5f9	244
narshu	0:fbfafa6bf5f9	245	//Updating state
narshu	0:fbfafa6bf5f9	246	X += col(K, 0) * Y;
narshu	0:fbfafa6bf5f9	247	X(2) = rectifyAng(X(2));
narshu	0:fbfafa6bf5f9	248
narshu	0:fbfafa6bf5f9	249	P = (I3 - K * H) * P;
narshu	0:fbfafa6bf5f9	250
narshu	0:fbfafa6bf5f9	251	statelock.unlock();
narshu	0:fbfafa6bf5f9	252
narshu	0:fbfafa6bf5f9	253	}
narshu	0:fbfafa6bf5f9	254
narshu	0:fbfafa6bf5f9	255	} else {
narshu	0:fbfafa6bf5f9	256	led4 = 1;
narshu	0:fbfafa6bf5f9	257	//printf("ERROR: in updateloop, code %#x", evt);
narshu	0:fbfafa6bf5f9	258	}
narshu	0:fbfafa6bf5f9	259
narshu	0:fbfafa6bf5f9	260	}
narshu	0:fbfafa6bf5f9	261
narshu	0:fbfafa6bf5f9	262	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

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

Kalman/Kalman.cpp@0:fbfafa6bf5f9, 2012-04-20 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning