Controller/ EKF tuning.

3 files changed, 23 insertions, 22 deletions

diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c
index 422e9b90e..3eed83b5c 100644
--- a/apps/examples/control_demo/params.c
+++ b/apps/examples/control_demo/params.c
@@ -15,16 +15,16 @@ PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass
 
 // stabilization mode
 PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.5f); // roll rate 2 aileron
-PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.5f); // pitch rate 2 elevator
+PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.2f); // pitch rate 2 elevator
 PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.2f); // yaw rate 2 rudder
 
 //  psi -> phi -> p
 PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f);      // heading 2 roll
 PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f);        // roll to roll rate
-PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.5f);  // roll limit, 28 deg
+PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f);  // roll limit, 28 deg
 
 // velocity -> theta
-PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.1f);      // velocity to pitch angle PID, prop gain
+PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f);      // velocity to pitch angle PID, prop gain
 PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f);      // integral gain
 PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f);      // derivative gain
 PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f);   // derivative low-pass
@@ -34,7 +34,7 @@ PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f);      // the min commanded pitch angle
 
 
 // theta -> q
-PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 2.0f);      // pitch angle to pitch-rate PID
+PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f);      // pitch angle to pitch-rate PID
 PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f);
@@ -49,7 +49,7 @@ PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f);
 
 // crosstrack
 PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f);  // cross-track to yaw angle limit 90 deg
-PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.002f);     // cross-track to yaw angle gain
+PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f);     // cross-track to yaw angle gain
 
 // speed command
 PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f);       // minimum commanded velocity
@@ -60,4 +60,4 @@ PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f);       // maximum commanded velocity
 PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f);     // trim aileron, normalized (-1,1)
 PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f);   // trim elevator (-1,1)
 PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f);     // trim rudder (-1,1)
-PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.81f);    // trim throttle (0,1)
+PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f);     // trim throttle (0,1)
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index d93a7bdc6..6acf39be8 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -66,7 +66,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	q(),
 	// subscriptions
 	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
-	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 1000), // limit to 1 Hz
+	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
 	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
 	// publications
 	_pos(&getPublications(), ORB_ID(vehicle_global_position)),
@@ -193,9 +193,10 @@ void KalmanNav::update()
 
 	// prediciton step
 	// using sensors timestamp so we can account for packet lag
-	float dt= (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
+	float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
 	//printf("dt: %15.10f\n", double(dtFast));
 	_predictTimeStamp = _sensors.timestamp;
+
 	// don't predict if time greater than a second
 	if (dt < 1.0f) {
 		predictState(dt);
@@ -213,7 +214,7 @@ void KalmanNav::update()
 	}
 
 	// attitude correction step
-	if (_sensors.timestamp - _attTimeStamp > 1e6 / 1) { // 1 Hz
+	if (_sensors.timestamp - _attTimeStamp > 1e6 / 20) { // 20 Hz
 		_attTimeStamp = _sensors.timestamp;
 		correctAtt();
 	}
@@ -228,9 +229,9 @@ void KalmanNav::update()
 	if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
 		_outTimeStamp = newTimeStamp;
 		printf("nav: %4d Hz, miss #: %4d\n",
-		       _navFrames / 10, _miss/ 10);
+		       _navFrames / 10, _miss / 10);
 		_navFrames = 0;
-		_miss= 0;
+		_miss = 0;
 	}
 }
 
@@ -325,11 +326,11 @@ void KalmanNav::predictState(float dt)
 	float lDot = vE / (cosLSing * R);
 	float rotRate = 2 * omega + lDot;
 	float vNDot = fN - vE * rotRate * sinL +
-			  vD * LDot;
+		      vD * LDot;
 	float vDDot = fD - vE * rotRate * cosL -
-			  vN * LDot + _g.get();
+		      vN * LDot + _g.get();
 	float vEDot = fE + vN * rotRate * sinL +
-			  vDDot * rotRate * cosL;
+		      vDDot * rotRate * cosL;
 
 	// rectangular integration
 	vN += vNDot * dt;
diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c
index 58ebeba3c..77225b4c7 100644
--- a/apps/examples/kalman_demo/params.c
+++ b/apps/examples/kalman_demo/params.c
@@ -1,16 +1,16 @@
 #include <systemlib/param/param.h>
 
 /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
-PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.1f);
-PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.1f);
-PARAM_DEFINE_FLOAT(KF_R_MAG, 0.1f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.1f);
+PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
+PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
+PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f);
-PARAM_DEFINE_FLOAT(KF_R_ACCEL, 0.1f);
-PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1.0f);
-PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 1.0f);
-PARAM_DEFINE_FLOAT(KF_ENV_G, 9.806f);
+PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
+PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
+PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
+PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f);
 PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
 PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);