aboutsummaryrefslogtreecommitdiff
path: root/apps
diff options
context:
space:
mode:
Diffstat (limited to 'apps')
-rw-r--r--apps/examples/control_demo/params.c36
-rw-r--r--apps/examples/kalman_demo/KalmanNav.cpp57
-rw-r--r--apps/examples/kalman_demo/params.c4
3 files changed, 57 insertions, 40 deletions
diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c
index 8f923f5a1..422e9b90e 100644
--- a/apps/examples/control_demo/params.c
+++ b/apps/examples/control_demo/params.c
@@ -24,40 +24,40 @@ PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate
PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.5f); // roll limit, 28 deg
// velocity -> theta
-PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.2f);
-PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f);
-PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f);
+PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.1f); // 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
+PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); // integrator wind up guard
+PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f); // the max commanded pitch angle
+PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // the min commanded pitch angle
// theta -> q
-PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f);
+PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 2.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);
PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f);
// h -> thr
-PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f); // altitude to throttle PID
PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f);
PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f);
PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f);
PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f);
// crosstrack
-PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // 90 deg
-PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.002f);
+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
// speed command
-PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f);
-PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f);
-PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f);
+PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f); // minimum commanded velocity
+PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f); // commanded velocity
+PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f); // maximum commanded velocity
// trim
-PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f);
-PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.81f);
+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)
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 240564b56..e804fbca5 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -141,8 +141,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
// HPos is constant
HPos(0, 3) = 1.0f;
HPos(1, 4) = 1.0f;
- HPos(2, 6) = 1.0f;
- HPos(3, 7) = 1.0f;
+ HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F;
+ HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F;
HPos(4, 8) = 1.0f;
// initialize all parameters
@@ -316,7 +316,11 @@ void KalmanNav::predictFast(float dt)
float cosL = cosf(lat);
float cosLSing = cosf(lat);
- if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
+ // prevent singularity
+ if (fabsf(cosLSing) < 0.01f) {
+ if (cosLSing > 0) cosLSing = 0.01;
+ else cosLSing = -0.01;
+ }
// position update
// neglects angular deflections in local gravity
@@ -577,11 +581,11 @@ void KalmanNav::correctAtt()
P = P - K * HAtt * P;
// fault detection
- float beta = y.dot(S.inverse() * y);
+ float beta = y.dot((S * S.transpose()).inverse() * y);
if (beta > 1000.0f) {
printf("fault in attitude: beta = %8.4f\n", (double)beta);
- //printf("y:\n"); y.print();
+ printf("y:\n"); y.print();
}
// update quaternions from euler
@@ -592,11 +596,11 @@ void KalmanNav::correctAtt()
void KalmanNav::correctPos()
{
using namespace math;
- Vector y(6);
+ Vector y(5);
y(0) = _gps.vel_n - vN;
y(1) = _gps.vel_e - vE;
- y(2) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat;
- y(3) = double(_gps.lon) / 1.0e7 / M_RAD_TO_DEG - lon;
+ y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
+ y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
y(4) = double(_gps.alt) / 1.0e3 - alt;
// update gps noise
@@ -604,12 +608,17 @@ void KalmanNav::correctPos()
float R = R0 + float(alt);
// prevent singularity
- if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
+ if (fabsf(cosLSing) < 0.01f) {
+ if (cosLSing > 0) cosLSing = 0.01;
+ else cosLSing = -0.01;
+ }
- float noiseLat = _rGpsPos.get() / R;
- float noiseLon = _rGpsPos.get() / (R * cosLSing);
- RPos(2, 2) = noiseLat * noiseLat;
- RPos(3, 3) = noiseLon * noiseLon;
+ float noiseVel = _rGpsVel.get();
+ float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
+ float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
+ float noiseAlt = _rGpsAlt.get();
+ RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7;
+ RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7;
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@@ -650,11 +659,16 @@ void KalmanNav::correctPos()
P = P - K * HPos * P;
// fault detetcion
- float beta = y.dot(S.inverse() * y);
+ float beta = y.dot((S * S.transpose()).inverse() * y);
if (beta > 1000.0f) {
printf("fault in gps: beta = %8.4f\n", (double)beta);
- //printf("y:\n"); y.print();
+ printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
+ double(y(0) / noiseVel),
+ double(y(1) / noiseVel),
+ double(y(2) / noiseLatDegE7),
+ double(y(3) / noiseLonDegE7),
+ double(y(4) / noiseAlt));
}
}
@@ -692,15 +706,18 @@ void KalmanNav::updateParams()
float R = R0 + float(alt);
// prevent singularity
- if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
+ if (fabsf(cosLSing) < 0.01f) {
+ if (cosLSing > 0) cosLSing = 0.01;
+ else cosLSing = -0.01;
+ }
float noiseVel = _rGpsVel.get();
- float noiseLat = _rGpsPos.get() / R;
- float noiseLon = _rGpsPos.get() / (R * cosLSing);
+ float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
+ float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
float noiseAlt = _rGpsAlt.get();
RPos(0, 0) = noiseVel * noiseVel; // vn
RPos(1, 1) = noiseVel * noiseVel; // ve
- RPos(2, 2) = noiseLat * noiseLat; // lat
- RPos(3, 3) = noiseLon * noiseLon; // lon
+ RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
+ RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon
RPos(4, 4) = noiseAlt * noiseAlt; // h
}
diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c
index 03cdca5ed..f36ac334f 100644
--- a/apps/examples/kalman_demo/params.c
+++ b/apps/examples/kalman_demo/params.c
@@ -1,8 +1,8 @@
#include <systemlib/param/param.h>
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
-PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
-PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
+PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f);
+PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f);
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-13 01:13:32 +0000