radiotester - Sample of program breaking when a certain set of …

Users » narshu » Code » radiotester

Shuto Naruse / Mbed 2 deprecated radiotester

Sample of program breaking when a certain set of source files are in a folder, but is fine when it is in the root. In this case, it is tested with RF12B.cpp, RF12B.h and rfdefs

Dependencies: mbed

Kalman/Kalman.cpp@0:349dc9b0984f, 2012-03-25 (annotated)

Committer:: narshu
Date:: Sun Mar 25 13:39:11 2012 +0000
Revision:: 0:349dc9b0984f

Who changed what in which revision?

User	Revision	Line number	New contents of line
narshu	0:349dc9b0984f	1	//***************************************************************************************
narshu	0:349dc9b0984f	2	//Kalman Filter implementation
narshu	0:349dc9b0984f	3	//***************************************************************************************
narshu	0:349dc9b0984f	4	#include "Kalman.h"
narshu	0:349dc9b0984f	5	//#include "rtos.h"
narshu	0:349dc9b0984f	6	//#include "RFSRF05.h"
narshu	0:349dc9b0984f	7	//#include "MatrixMath.h"
narshu	0:349dc9b0984f	8	//#include "Matrix.h"
narshu	0:349dc9b0984f	9	//#include "math.h"
narshu	0:349dc9b0984f	10	//#include "globals.h"
narshu	0:349dc9b0984f	11	//#include "motors.h"
narshu	0:349dc9b0984f	12	//#include "system.h"
narshu	0:349dc9b0984f	13
narshu	0:349dc9b0984f	14
narshu	0:349dc9b0984f	15	Kalman::Kalman() : //Motors &motorsin) :
narshu	0:349dc9b0984f	16	//X(3,1),
narshu	0:349dc9b0984f	17	//P(3,3),
narshu	0:349dc9b0984f	18	//Q(3,3),
narshu	0:349dc9b0984f	19	//sonararray(p13,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9){
narshu	0:349dc9b0984f	20	rflol(p5, p6, p7, p8, p9){
narshu	0:349dc9b0984f	21	//motors(motorsin) {
narshu	0:349dc9b0984f	22	//predictthread(predictloopwrapper, this),
narshu	0:349dc9b0984f	23	//predictticker( SIGTICKARGS(predictthread, 0x1) ), //(tfuncptr_t) (&Signalsetter::callback), osTimerPeriodic, (void*)(new Signalsetter(predictthread, 0x1)) ),
narshu	0:349dc9b0984f	24	//sonarthread(sonarloopwrapper, this),
narshu	0:349dc9b0984f	25	//sonarticker( SIGTICKARGS(sonarthread, 0x1) ){
narshu	0:349dc9b0984f	26	//updatethread(updateloopwrapper, this) {
narshu	0:349dc9b0984f	27
narshu	0:349dc9b0984f	28
narshu	0:349dc9b0984f	29	//Initilising matrices
narshu	0:349dc9b0984f	30	/*
narshu	0:349dc9b0984f	31	X << 500
narshu	0:349dc9b0984f	32	<< 0
narshu	0:349dc9b0984f	33	<< 0;
narshu	0:349dc9b0984f	34
narshu	0:349dc9b0984f	35	P << 1000 << 0 << 0
narshu	0:349dc9b0984f	36	<< 0 << 1000 << 0
narshu	0:349dc9b0984f	37	<< 0 << 0 << 1000;
narshu	0:349dc9b0984f	38
narshu	0:349dc9b0984f	39	Q << 1 << 0 << 0 //temporary matrix
narshu	0:349dc9b0984f	40	<< 0 << 1 << 0
narshu	0:349dc9b0984f	41	<< 0 << 0 << 1;
narshu	0:349dc9b0984f	42
narshu	0:349dc9b0984f	43	*/
narshu	0:349dc9b0984f	44	R = 100; //Preliminary
narshu	0:349dc9b0984f	45
narshu	0:349dc9b0984f	46	//attach callback
narshu	0:349dc9b0984f	47	//sonararray.callbackobj = (DummyCT*)this;
narshu	0:349dc9b0984f	48	//sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance)) &Kalman::runupdate;
narshu	0:349dc9b0984f	49
narshu	0:349dc9b0984f	50	//attach predict ticker
narshu	0:349dc9b0984f	51	//predictticker.attach(this, &Kalman::predictlauncher, 0.5); //20Hz for now
narshu	0:349dc9b0984f	52
narshu	0:349dc9b0984f	53	//predictticker.start(5000);
narshu	0:349dc9b0984f	54
narshu	0:349dc9b0984f	55	//predictthread = new Thread(predictloopwrapper, this);
narshu	0:349dc9b0984f	56
narshu	0:349dc9b0984f	57	}
narshu	0:349dc9b0984f	58
narshu	0:349dc9b0984f	59	/*
narshu	0:349dc9b0984f	60	void Kalman::predictloop() {
narshu	0:349dc9b0984f	61	while (1) {
narshu	0:349dc9b0984f	62	Thread::signal_wait(0x1);
narshu	0:349dc9b0984f	63	predict();
narshu	0:349dc9b0984f	64	}
narshu	0:349dc9b0984f	65	}
narshu	0:349dc9b0984f	66	*/
narshu	0:349dc9b0984f	67
narshu	0:349dc9b0984f	68	/*
narshu	0:349dc9b0984f	69	void Kalman::predict() {
narshu	0:349dc9b0984f	70
narshu	0:349dc9b0984f	71	static int lastleft = 0;
narshu	0:349dc9b0984f	72	static int lastright = 0;
narshu	0:349dc9b0984f	73
narshu	0:349dc9b0984f	74	int left = motors.encoderToDistance(motors.getEncoder1()) - lastleft;
narshu	0:349dc9b0984f	75	int right = motors.encoderToDistance(motors.getEncoder2()) - lastright;
narshu	0:349dc9b0984f	76
narshu	0:349dc9b0984f	77	lastleft += left;
narshu	0:349dc9b0984f	78	lastright += right;
narshu	0:349dc9b0984f	79
narshu	0:349dc9b0984f	80	float dxp, dyp;
narshu	0:349dc9b0984f	81
narshu	0:349dc9b0984f	82	//The below calculation are in body frame (where +x is forward)
narshu	0:349dc9b0984f	83	float thetap = (right - left)PI / (2.0f robotCircumference);
narshu	0:349dc9b0984f	84
narshu	0:349dc9b0984f	85	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:349dc9b0984f	86	float d = (right + left)/2.0f;
narshu	0:349dc9b0984f	87	dxp = d*cos(thetap/2);
narshu	0:349dc9b0984f	88	dyp = d*sin(thetap/2);
narshu	0:349dc9b0984f	89	} else { //calculate circle arc
narshu	0:349dc9b0984f	90	//float r = (right + left) / (4.0f * PI * thetap);
narshu	0:349dc9b0984f	91	float r = (right+left) / (2.0f*thetap);
narshu	0:349dc9b0984f	92	dxp = abs(r)*sin(thetap);
narshu	0:349dc9b0984f	93	dyp = r - r*cos(thetap);
narshu	0:349dc9b0984f	94	}
narshu	0:349dc9b0984f	95
narshu	0:349dc9b0984f	96	statelock.lock();
narshu	0:349dc9b0984f	97
narshu	0:349dc9b0984f	98	//rotating to cartesian frame and updating state
narshu	0:349dc9b0984f	99	X(1,1) += dxp * cos(X(3,1)) - dyp * sin(X(3,1));
narshu	0:349dc9b0984f	100	X(2,1) += dxp * sin(X(3,1)) + dyp * cos(X(3,1));
narshu	0:349dc9b0984f	101	X(3,1) += thetap;
narshu	0:349dc9b0984f	102
narshu	0:349dc9b0984f	103	//Linearising F around X
narshu	0:349dc9b0984f	104	Matrix F(3,3);
narshu	0:349dc9b0984f	105	F << 1 << 0 << (dxp * -sin(X(3,1)) - dyp * cos(X(3,1)))
narshu	0:349dc9b0984f	106	<< 0 << 1 << (dxp * cos(X(3,1)) - dyp * sin(X(3,1)))
narshu	0:349dc9b0984f	107	<< 0 << 0 << 1;
narshu	0:349dc9b0984f	108
narshu	0:349dc9b0984f	109	//Updating P
narshu	0:349dc9b0984f	110	P = F * P * MatrixMath::Transpose(F) + Q;
narshu	0:349dc9b0984f	111
narshu	0:349dc9b0984f	112	statelock.unlock();
narshu	0:349dc9b0984f	113	}
narshu	0:349dc9b0984f	114
narshu	0:349dc9b0984f	115
narshu	0:349dc9b0984f	116	void Kalman::sonarloop() {
narshu	0:349dc9b0984f	117	while (1) {
narshu	0:349dc9b0984f	118	Thread::signal_wait(0x1);
narshu	0:349dc9b0984f	119	sonararray.startRange();
narshu	0:349dc9b0984f	120	}
narshu	0:349dc9b0984f	121	}
narshu	0:349dc9b0984f	122
narshu	0:349dc9b0984f	123
narshu	0:349dc9b0984f	124	void Kalman::runupdate(int sonarid, float dist) {
narshu	0:349dc9b0984f	125	measurmentdata* measured = NULL;//(measurmentdata*)measureMQ.alloc();
narshu	0:349dc9b0984f	126	if (measured) {
narshu	0:349dc9b0984f	127	measured->mtype = (measurement_t)sonarid;
narshu	0:349dc9b0984f	128	measured->value = dist;
narshu	0:349dc9b0984f	129	int putret = NULL;//osStatus putret = measureMQ.put(measured);
narshu	0:349dc9b0984f	130	if (putret)
narshu	0:349dc9b0984f	131	printf("putting in MQ error code %#x\r\n", putret);
narshu	0:349dc9b0984f	132	} else {
narshu	0:349dc9b0984f	133	printf("MQalloc returned NULL ptr\r\n");
narshu	0:349dc9b0984f	134	}
narshu	0:349dc9b0984f	135	}
narshu	0:349dc9b0984f	136
narshu	0:349dc9b0984f	137	void Kalman::updateloop() {
narshu	0:349dc9b0984f	138
narshu	0:349dc9b0984f	139	while (1) {
narshu	0:349dc9b0984f	140
narshu	0:349dc9b0984f	141	osEvent evt = measureMQ.get();
narshu	0:349dc9b0984f	142	if (evt.status == osEventMail) {
narshu	0:349dc9b0984f	143
narshu	0:349dc9b0984f	144	measurmentdata& measured = (measurmentdata)evt.value.p;
narshu	0:349dc9b0984f	145	int sonarid = measured.mtype; //Note, may support more measurment types than sonar in the future!
narshu	0:349dc9b0984f	146	float dist = measured.value;
narshu	0:349dc9b0984f	147
narshu	0:349dc9b0984f	148	statelock.lock();
narshu	0:349dc9b0984f	149
narshu	0:349dc9b0984f	150	float rbx = X(1,1) - beaconpos[sonarid].x;
narshu	0:349dc9b0984f	151	float rby = X(2,1) - beaconpos[sonarid].y;
narshu	0:349dc9b0984f	152
narshu	0:349dc9b0984f	153	float expecdist = sqrt(rbxrbx + rbyrby);
narshu	0:349dc9b0984f	154	float Y = dist - expecdist;
narshu	0:349dc9b0984f	155
narshu	0:349dc9b0984f	156	float dhdx = rbx / expecdist;
narshu	0:349dc9b0984f	157	float dhdy = rby / expecdist;
narshu	0:349dc9b0984f	158	Matrix H(1,3);
narshu	0:349dc9b0984f	159	H << dhdx << dhdy << 0;
narshu	0:349dc9b0984f	160
narshu	0:349dc9b0984f	161	Matrix PH = P * MatrixMath::Transpose(H);
narshu	0:349dc9b0984f	162	float S = (H * PH)(1,1) + R;
narshu	0:349dc9b0984f	163	Matrix K = PH * (1/S);
narshu	0:349dc9b0984f	164
narshu	0:349dc9b0984f	165	//Updating state
narshu	0:349dc9b0984f	166	X += K*Y;
narshu	0:349dc9b0984f	167
narshu	0:349dc9b0984f	168
narshu	0:349dc9b0984f	169	Matrix I3(3,3);
narshu	0:349dc9b0984f	170	I3 << 1 << 0 << 0
narshu	0:349dc9b0984f	171	<< 0 << 1 << 0
narshu	0:349dc9b0984f	172	<< 0 << 0 << 1;
narshu	0:349dc9b0984f	173	P = (I3 - K * H) * P;
narshu	0:349dc9b0984f	174
narshu	0:349dc9b0984f	175	statelock.unlock();
narshu	0:349dc9b0984f	176
narshu	0:349dc9b0984f	177	printf("u %.1f %.1f %.1f\r\n", X(1,1), X(2,1), X(3,1));
narshu	0:349dc9b0984f	178	} else {
narshu	0:349dc9b0984f	179	printf("ERROR: in updateloop, code %#x", evt);
narshu	0:349dc9b0984f	180	}
narshu	0:349dc9b0984f	181
narshu	0:349dc9b0984f	182	}
narshu	0:349dc9b0984f	183
narshu	0:349dc9b0984f	184	}
narshu	0:349dc9b0984f	185	*/

Repository toolbox

Export to desktop IDE

Build repository

Repository details

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

Kalman/Kalman.cpp@0:349dc9b0984f, 2012-03-25 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning