To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit a650984f authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

Made quite some adjustments to the overall performance

parent f058e11b
......@@ -21,8 +21,8 @@ defaultSetpoint: [0.0, 0.0, 0, 0.0]
#take off and landing parameters (in meters and seconds)
takeOffDistance : 0.4
landingDistance : -0.5
durationTakeOff : 3
durationLanding : 3
#durationTakeOff : 3
#durationLanding : 3
# Liner Trayectory following parameters
#distanceThreshold : 0.5
......@@ -41,7 +41,7 @@ feedforwardMotor: [43000, 43000, 43000, 43000]
gainMatrixRoll: [0, -1.3, 0, 0, -0.9, 0, 4.715369735, 0, 0]
gainMatrixPitch: [1.3, 0, 0, 0.9, 0, 0, 0, 4.715369735, 0]
#gainMatrixYaw: [0, 0, 0, 0, 0, 0, 0, 0, 2.843099534]
gainMatrixThrust: [0, 0, 0.20195826, 0, 0, 0.08362477, 0, 0, 0]
gainMatrixThrust: [0, 0, 0.15, 0, 0, 0.06, 0, 0, 0]
#kalman filter
#filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
......@@ -53,8 +53,8 @@ gainMatrixThrust: [0, 0, 0.20195826, 0, 0, 0.08362477, 0, 0, 0]
#take off and landing parameters (in meters and seconds)
#takeOffDistance : 0.4
#landingDistance : -0.5
#durationTakeOff : 3
#durationLanding : 3
durationTakeOff : 1
durationLanding : 2
# Liner Trayectory following parameters
distanceThreshold : 0.3
......@@ -40,6 +40,9 @@ using namespace d_fall_pps;
#define DISTANCE_ERROR_TRESHOLD 0.9
#define ANCHOR_INDEX_TOP 4
#define ANCHOR_HEIGHT_TOP 17.0 //anchors.data[4].z //in dm
// the clientID defines namespace and identifier in the CentralManagerService
int clientID;
......@@ -255,7 +258,7 @@ void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, doubl
lastPrint = thisPrint;
}
//filterDistances(dist);
filterDistances(dist);
/*for(int i = 0; i < UWB_NUM_ANCHORS; ++i)
{
......@@ -264,12 +267,14 @@ void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, doubl
IndoorNewton(anchorPositions, dist, result, 50);
//ROS_INFO_STREAM(result[0]);
double delat_x_2 = (anchorPositions[3*ANCHOR_INDEX_TOP] - result[0]) * (anchorPositions[3*ANCHOR_INDEX_TOP] - result[0]);
double delta_y_2 = (anchorPositions[3*ANCHOR_INDEX_TOP+1] - result[1]) * (anchorPositions[3*ANCHOR_INDEX_TOP+1] - result[1]);
double cf_height = std::sqrt(std::abs(dist[ANCHOR_INDEX_TOP]*dist[ANCHOR_INDEX_TOP] - delat_x_2 - delta_y_2));
result[2] = ANCHOR_HEIGHT_TOP - cf_height;
updateKalmanState(xState, result[0] / 10.0);
updateKalmanState(yState, result[1] / 10.0);
updateKalmanState(zState, result[2] / 10.0);
// Use this for Kalmanfiltered position
xyz[0] = xState.x;
......@@ -283,7 +288,7 @@ void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, doubl
// xyz[1] = result[1] / 10.0;
//xyz[2] = result[2] / 10.0;
publishLog(distances, dist);
//publishLog(distances, dist);
}
void d_fall_pps::publishLog(const std_msgs::UInt32MultiArray& distances, const double* dist)
......@@ -336,17 +341,16 @@ void d_fall_pps::initKalmanStates()
yState.p = 0;
// Init z state
// TODO Update
zState.x = 0;
zState.q = 5e-6; //KALMAN_P_VARIANCE;
zState.r = 5e-4; //KALMAN_M_VARIANCE;
zState.q = 5e-5;//1e-3;//5e-6; //KALMAN_P_VARIANCE;
zState.r = 5e-6;//1;//5e-4; //KALMAN_M_VARIANCE;
zState.p = 0;
for(unsigned int i = 0; i < UWB_NUM_ANCHORS; i++)
{
kalmanDist[i].x = 0;
kalmanDist[i].q = 8;
kalmanDist[i].r = 500;
kalmanDist[i].r = 100;//500;
kalmanDist[i].p = 0;
}
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment