APPENDIX D: STATE ESTIMATION SOFTWARE


/********************************************************************************
*										
*	est.c
*
*	A state estimator for Ranger NBV.
*
*	Written by Joe Graves August 1994
*										
*********************************************************************************/
#include <vxWorks.h>
#include <taskLib.h>
#include <math.h>
#include <stdio.h>
#include <asiports.h>
#include <pivecs.h>
#include <dynamics_jdg.h>
#include <rotation_jdg.h>
#include <vector_jdg.h>
#include <sys.h>
#include <nbv_cs_newer.h>
#include <nbv_common.h>
#include "est.h"
#include "../fltTCX/fltTCX.h"
#define ESC	27
/****************************************
*					*
*	Function Prototypes		*
*					*
*****************************************/
void		estTerminate(void);
Vector		estNonlinearAdaptiveObserver(Vector, Vector);
Vector		estSimpleEstimator(void);
Vector		estObserverControlLaw(Vector, Vector);
Vector		estQuatFromAngleAxis(Scalar, Vector);
Scalar		estAngleFromQuat(Vector);
Vector		estAxisFromQuat(Vector);
/****************************************
*					*
*	Module Status Globals		*
*					*
*****************************************/
extern Module_State	SYS_Module_State[MAXIMUM_ALLOWED_MODULES];
/****************************************
*					*
*	Input Globals			*
*					*
*****************************************/
extern Vector	IMU_Data;
extern Vector	ESU_Magnetometer_Data;
extern Vector	IMU_Acceleration;
extern Vector	IMU_Angular_Rate;
extern Vector	IMU_Angular_Accel;
extern Vector	CON_Des_Quat;
extern Vector	CON_Quat_Err;
extern Vector	CON_Omega_Err;
extern Vector	CON_Tau;
extern Vector	CON_a;
extern Vector	THR_Hand_Controllers;
extern int	CON_Simulate;
extern Vector	CON_Simulate_Ang_Bias;
extern Vector	CON_Control_Ang_Vel;
extern float	BCS_Pos[6];
/****************************************
*					*
*	Output Globals			*
*					*
*****************************************/
Vector		EST_Quat;
Vector		EST_Omega;
Vector		EST_State;
/****************************************
*					*
*	Internal Globals		*
*					*
*****************************************/
static SEM_ID		EST_Sem;
Scalar			EST_QuatFilter;
/****************************************
*					*
*	External Globals		*
*					*
*****************************************/
int estMain( void)
{
int	count = 0, count2 = 0;
Scalar	s1;
	SYS_Module_State[EST].Done = FALSE;
	printf("State Estimator Started\n\r");
	/* set to 50 Hz running rate */
	EST_Sem = semBCreate( SEM_Q_FIFO, SEM_EMPTY);
	sysTaskManage( EST_Sem, 50, 0);
	EST_Quat = VectorInit(4,0.0,0.0,0.0,1.0);
	EST_State = VectorInit(13,0.0,0.0,0.0,
				  0.0,0.0,0.0,
				  0.0,0.0,0.0,1.0,
				  .1, 0.0, 0.0);
	EST_QuatFilter = 0.;
	count = 0; count2 = 0;
	SYS_Module_State[EST].Ready = TRUE;
/***	Main loop	***/
	while( !SYS_Module_State[EST].Done){
		semTake( EST_Sem, WAIT_FOREVER);
		sysSetStatus(EST, MODULE_RUNNING);
		/*if(Dim(IMU_Data)==6) EST_Quat=estNonlinearAdaptiveObserver(EST_Quat, IMU_Data);*/
		if(!CON_Simulate) {
		    if(Dim(IMU_Data)==6) EST_Quat=estSimpleEstimator();
		    EST_State = VectorInit(13,0.0,0.0,0.0,
					  0.0,0.0,0.0,
					  V(EST_Quat,1),V(EST_Quat,2),V(EST_Quat,3),V(EST_Quat,4),
					  V(IMU_Angular_Rate,1), V(IMU_Angular_Rate,2), V(IMU_Angular_Rate,3));
		    /*PrintV(IMU_Data);printf("Quat\r");*/
		} else { 
		    EST_Quat = VectorInit(4, V(EST_State, 7), V(EST_State, 8), V(EST_State, 9), V(EST_State, 10));		    
		    /*IMU_Angular_Rate = VectorInit(3, V(EST_State, 11)+V(CON_Simulate_Ang_Bias, 1),
						     V(EST_State, 12)+V(CON_Simulate_Ang_Bias, 2),
						     V(EST_State, 13)+V(CON_Simulate_Ang_Bias, 3));*/
		}
		count2++;
		if(count2==5) { /* Reply to cs at 10 Hz */
			count2=0;
			fltQuatErrSend(CON_Quat_Err);
		}
		fltEstQuatSend(EST_Quat);
		fltDesQuatSend(CON_Des_Quat);
		count++;
		if(count==10) { /* Reply to cs at 5 Hz */
			count=0;
			fltMagnetometerSend(ESU_Magnetometer_Data);
			
			/*fltAccelerometerSend(VectorInit(3,V(IMU_Data,1),V(IMU_Data,2),V(IMU_Data,3)));*/
			fltAccelerometerSend(VectorInit(3,V(CON_a, 1), V(CON_a, 4), V(CON_a, 6)));
			/*fltRateSend(VectorInit(3,V(CON_Control_Ang_Vel,1),V(CON_Control_Ang_Vel,2),V(CON_Control_Ang_Vel,3)));*/
			fltRateSend(VectorInit(3,V(CON_a, 7), V(CON_a, 8), V(CON_a, 9)));
			/*fltOmegaErrSend(CON_Omega_Err);*/
			fltOmegaErrSend(VectorInit(3,(Scalar)BCS_Pos[1],(Scalar)BCS_Pos[3],(Scalar)BCS_Pos[4]));
			/*fltOmegaErrSend(VectorInit(3, V(CON_a, 7), V(CON_a, 8), V(CON_a, 9)));*/
			fltTauSend(VectorInit(3, MagnitudeV(CON_Tau), MagnitudeV(THR_Hand_Controllers), 0.)  );
			/*fltTauSend(VectorInit(3, V(CON_a, 1), V(CON_a, 4), V(CON_a, 6)));*/
			/*PrintV(CON_a);printf("\r");*/
			/*PrintV(IMU_Data);printf("\r");*/
			/*estTCX();*/
		}
	}
    sysSetStatus(EST, MODULE_DELETED);
	estTerminate();
	return(0);
}
/* estTerminate
 *
 *	Function used to end program operation in  "normal" way at the moment it
 *	only calls exit(), but in the future it should also make sure that any
 *	relevant data is saved or that the user is notfied of any errors, etc...
 */
void estTerminate( void)
{
	printf("estTerminate Called\n\r");
	sysTaskUnmanage( EST_Sem);
	semDelete( EST_Sem);
	/*estEndTCX();*/
	SYS_Module_State[EST].Ready = FALSE;
}
Vector	estNonlinearAdaptiveObserver(Vector qu, Vector sens)
{
Scalar	angleErr;
Vector	x, tmp, omega, gSens, gCalc, axis, qError, qErrorDot;
matrix  T;
static Vector	qErrorOld, tttt, offset;
	if( Dim(tttt) == 0 ) tttt=VectorInit(3,0.0,0.0,0.0);
	if( Dim(offset) == 0 ) offset=VectorInit(3,0.012,0.062,-0.082);
	/*PrintV(offset); printf("\r");*/
	V(x,1)=V(sens,1);
	V(x,2)=V(sens,2);
	V(x,3)=V(sens,3);
/* The next three lines put the rate sensor readings into the vector that will be returned from
 this subroutine.  The rate offset vector is added to the rates from the sensor.  The offset vector
 is the best estimate of the zero bias in the rate sensor values.  The offset vector is in the vehicle
 coordinate frame */
	V(x,4)=V(sens,4)+V(offset,1);
	V(x,5)=V(sens,5)+V(offset,2);
	V(x,6)=V(sens,6)+V(offset,3);
 
/* Put the best estimate of the angular velocity vector (in vehicle coordinates) into the vector omega */
	omega = VectorInit(3,V(x,4),V(x,5),V(x,6));
	EST_Omega = omega;
	/*PrintV(sens); printf("\r");*/
/* Integrate the quaternion. (0.05 sec time step) */
	qu = AddVV(qu,ScalarMultSV(0.05,DQuatDt(qu, omega)));
/* Normalize the quaternion. (ensure that the magnitude stays equal to one) */
	qu = ScalarMultSV(1.0/MagnitudeV(qu),qu);
/* Temporary bogus integration that is easier to look at than a quaternion  This is
	not part of the algorithm.. It is just used for visual output  */
	tttt = AddVV(tttt,ScalarMultSV(0.05,omega));
/* A unit vector in the direction of the current accelerometer triad reading.  This is temporarily
 assumed to be straight down */
	gSens = UnitV(VectorInit(3,V(x,1),V(x,2),V(x,3)));
/*	PrintV(gSens);*/
/* Calculate where the vehicle thinks that the gravity vector should be.  For now this only takes into
 account the attitude of the vehicle i.e. (the quaternion vector) */ 
	
	gCalc = MultMV(RotFromQuat(qu),VectorInit(3,0.0,0.0,1.0));
/* Find the angle between the sensed gravity vector, and the calculated gravity vector */
	angleErr = AngleVV(gSens,gCalc);
/* Find the axis of rotation between the sensed and calculated gravity vectors */
	axis = CrossVV(gSens,gCalc);
	if(MagnitudeV(axis) < 0.000001)  /* this prevents a division by zero error */
		axis = VectorInit(3,0.0,1.0,0.0); /* arbitrary axis of rotation */
	else
		axis = UnitV(axis);
/* Express the rotation from the sensed to the calculated gravity vector as a quaternion */
	qError = estQuatFromAngleAxis(angleErr,axis);
/* Take the first derivative of the quaternion error */
	if(Dim(qErrorOld)==0) qErrorOld = qError;	/* first time through */
	qErrorDot = ScalarMultSV(1.0/0.04545,AddVV(qError,ScalarMultSV(-1.0,qErrorOld)));
/* Refine the estimate of the rate offset vector as a function of the difference between the sensed
 and calculated gravity vectors, and also as a function of the rate that the difference is changing */
/*	PrintS(angleErr); printf("\n\r");
	PrintV(estObserverControlLaw(qError,qErrorDot)); printf("\r");*/
	offset = AddVV(offset,estObserverControlLaw(qError,qErrorDot));
/* This is used to find the rate that the gravity vector error is changing */
	qErrorOld = qError;
	x=qu;
/*	PrintV(ScalarMultSV(180.0/3.14159265,tttt)); printf("\r");*/
/*	PrintS(angleErr/3.14159265*180.0);printf("\n\r");*/
/*	fprintf(fp2, "%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n",V(x,1),V(x,2),V(x,3),V(x,4),V(x,5),V(x,6));*/	
	return(x);	
}
Vector	estSimpleEstimator(void)
{
Vector	x;
Vector	vx,vy,vz;
matrix	mm;
Vector	aPerturb;
Vector	r;
static Scalar	ang, angAv;
static Vector	axis, axisAv;
static int	running = 0;
	if(!running) {
	    running = 1;
	    axisAv = VectorInit(3, 0., 0., 0.);
	    angAv = 0.;
	}
	
	r = VectorInit(3, 1.2, -0.15, -.1);
	vz = UnitV(IMU_Acceleration);
/*
	aPerturb = CrossVV(IMU_Angular_Rate, CrossVV(IMU_Angular_Rate, r));
	aPerturb = AddVV(aPerturb, CrossVV(IMU_Angular_Accel, r));
	vz = AddVV(vz, ScalarMultSV(-1., aPerturb));
*/
	vy = UnitV(CrossVV(vz,ESU_Magnetometer_Data));
	vx = UnitV(CrossVV(vy,vz));
	/* now describe as a rotation from tank to vehicle coordinates */	
	mm = matrixInit(3,3,V(vx,1),V(vy,1),V(vz,1),
			     V(vx,2),V(vy,2),V(vz,2),
			     V(vx,3),V(vy,3),V(vz,3));
	x = QuatFromRot(TransposeM(mm));
	ang = estAngleFromQuat(x);
	axis = estAxisFromQuat(x);
	angAv = ((1.-EST_QuatFilter)*ang)+(EST_QuatFilter*angAv);
	ang = angAv;
	axisAv = AddVV(ScalarMultSV((1.-EST_QuatFilter), axis), ScalarMultSV(EST_QuatFilter, axisAv));
	axis = axisAv;
	x = estQuatFromAngleAxis(angAv, axisAv);
	return(x);	
}
Vector	estObserverControlLaw(Vector qerr, Vector qerrdot)
{
Vector	x;
	if(V(qerr,4) < 0.0) ScalarMultSV(-1.0,qerr);
	
	V(x,1) = V(qerr,1)*(+.001)+V(qerrdot,1)*(0.01);
	V(x,2) = V(qerr,2)*(+.001)+V(qerrdot,2)*(0.01);
	V(x,3) = V(qerr,3)*(+.001)+V(qerrdot,3)*(0.01);
	Dim(x) = 3;
	return(x);
}
Vector	estQuatFromAngleAxis(Scalar ang, Vector ax)
{
Vector	x;
	ax = UnitV(ax);
	x = UnitV(VectorInit(4,V(ax,1)*sin(ang/2.0),V(ax,2)*sin(ang/2.0),
					V(ax,3)*sin(ang/2.0),cos(ang/2.0)));
	return(x);
}
Scalar	estAngleFromQuat(Vector q)
{
Scalar	x;
	x = 2.*acos(V(q, 4));
	return(x);
}
Vector	estAxisFromQuat(Vector q)
{
Vector	x;
Scalar	halfang;
	halfang = acos(V(q, 4));
	x = VectorInit(3, V(q, 1)/sin(halfang), V(q, 2)/sin(halfang), V(q, 3)/sin(halfang));
	return(x);
}
/* end of file */