Telemetry Parameters
From OrbSWARM
Contents
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;