Telemetry Parameters

feedback.c

Lateral PID - lateralPID.Kp, lateralPID.Ki, lateralPID.Kd

Velocity PID - velocityPID.Kp, velocityPID.Ki, velocityPID.Kd

pathfollow.c

PATHCARROTDISTANCE (currently #define)

kalmanswarm.c

stateEstimate Initialization

Important Ones:

 stateEstimate->psi 	= -PI/5; 	// heading / yaw
 stateEstimate->x 		= 554159.9;  // meters East
 stateEstimate->y 		= 4177058.3;	// meters North

Full Set:

 stateEstimate->vdot 	= 0.0;	// acceleration
 stateEstimate->v 		= 0.0;  // velocity
 stateEstimate->phidot = 0.0;	// roll angle rate
 stateEstimate->phi 	= 0.0;  // roll angle
 stateEstimate->psi 	= -PI/5; 	// heading / yaw
 stateEstimate->theta 	= 0.0;  // frontward pitch
 stateEstimate->x 		= 554159.9;  // meters East
 stateEstimate->y 		= 4177058.3;	// meters North
 stateEstimate->xab	= 0.86;	// x accelerometer bias
 stateEstimate->yab	= 1.52;	// y accelerometer bias
 stateEstimate->zab	= -1.87;	// z accelerometer bias
 stateEstimate->xrb	= -0.26;	// x rate gyro bias
 stateEstimate->zrb	= 2.316;	// z rate gyro bias
 stateEstimate->yawb	= 0.0;	// z rate gyro bias

State Expected Errors

 Qk[ STATE_vdot ][ STATE_vdot ] 	= 0.0001;
 Qk[ STATE_v ][ STATE_v ] 			= 0.0;
 Qk[ STATE_phidot ][ STATE_phidot ]= 0.01;
 Qk[ STATE_phi ][ STATE_phi ] 		= 0.0;
 Qk[ STATE_theta ][ STATE_theta ] 	= 0.002;
 Qk[ STATE_psi ][ STATE_psi ] 		= 0.0000001;
 Qk[ STATE_x ][ STATE_x ] 			= 0.0;
 Qk[ STATE_y ][ STATE_y ] 			= 0.0;
 Qk[ STATE_xab ][ STATE_xab ] 		= 0.000005;
 Qk[ STATE_yab ][ STATE_yab ] 		= 0.00005;
 Qk[ STATE_zab ][ STATE_zab ] 		= 0.000005;
 Qk[ STATE_xrb ][ STATE_xrb ] 		= 0.00002;
 Qk[ STATE_zrb ][ STATE_zrb ]  	= 0.00002;
 Qk[ STATE_yawb ][ STATE_yawb ]  	= 0.0000000003;

Sensor Expected Errors

 R[ MEAS_xa ][ MEAS_xa ] 		= 1.0;
 R[ MEAS_ya ][ MEAS_ya ] 		= 1.0;
 R[ MEAS_za ][ MEAS_za ] 		= 1.0;
 R[ MEAS_xr ][ MEAS_xr ] 		= 0.1;
 R[ MEAS_zr ][ MEAS_zr ] 		= 0.2;
 R[ MEAS_xg ][ MEAS_xg ] 		= 2.0;
 R[ MEAS_yg ][ MEAS_yg ] 		= 2.0;
 R[ MEAS_psig ][ MEAS_psig ] 	= PI/2.0;
 R[ MEAS_vg ][ MEAS_vg ] 		= 0.5;
 R[ MEAS_omega ][ MEAS_omega ] = 0.1;
 R[ MEAS_yaw ][ MEAS_yaw ] 	= 0.003;