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 97438929 authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

Debug and tune

parent 00cbae1e
......@@ -30,7 +30,7 @@
<!-- <param name="studentID" value="$(optenv ROS_NAMESPACE)" />
</node>-->
<node pkg="d_fall_pps" name="student_GUI" output="screen" type="student_GUI">
<node pkg="d_fall_pps" name="student_GUI" output="screen" type="student_GUI" required="true">
<!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
</node>
......
################### VICON Settings
#equlibrium offset
feedforwardMotor: [41000, 41000, 41000, 41000]
#feedforwardMotor: [41000, 41000, 41000, 41000]
#quadratic motor regression equation (a0, a1, a2)
motorPoly: [5.484560e-4, 1.032633e-6, 2.130295e-11]
#feedback gain matrix
#feedback gain matrix original
#gainMatrixRoll: [0, -1.714330725, 0, 0, -1.337107465, 0, 5.115369735, 0, 0]
#gainMatrixPitch: [1.714330725, 0, 0, 1.337107465, 0, 0, 0, 5.115369735, 0]
gainMatrixYaw: [0, 0, 0, 0, 0, 0, 0, 0, 2.843099534]
gainMatrixThrust: [0, 0, 0.22195826, 0, 0, 0.12362477, 0, 0, 0]
#gainMatrixThrust: [0, 0, 0.22195826, 0, 0, 0.12362477, 0, 0, 0]
#kalman filter
filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
#setpoint in meters (x, y, z, yaw)
defaultSetpoint: [0.0, 0.0, 0.4, 0.0]
defaultSetpoint: [0.0, 0.0, 0, 0.0]
#take off and landing parameters (in meters and seconds)
takeOffDistance : 0.4
......@@ -31,17 +31,17 @@ durationLanding : 3
################### UWB Settings
#equlibrium offset
#feedforwardMotor: [46000, 46000, 46000, 46000]
feedforwardMotor: [43000, 43000, 43000, 43000]
#quadratic motor regression equation (a0, a1, a2)
#motorPoly: [5.484560e-4, 1.032633e-6, 2.130295e-11]
#feedback gain matrix
#feedback gain matrix changed
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.22195826, 0, 0, 0.12362477, 0, 0, 0]
gainMatrixThrust: [0, 0, 0.20195826, 0, 0, 0.08362477, 0, 0, 0]
#kalman filter
#filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
......
......@@ -185,11 +185,11 @@ void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
// Uncomment the lines to be overriden by vicon when using UWB
//CFData_uwb.x = CFData_vicon.x;
//CFData_uwb.y = CFData_vicon.y;
CFData_uwb.z = CFData_vicon.z;
//CFData_uwb.z = CFData_vicon.z;
//CFData_uwb.roll = CFData_vicon.roll;
//CFData_uwb.pitch = CFData_vicon.pitch;
CFData_uwb.pitch *= -1;
CFData_uwb.yaw = CFData_vicon.yaw;
//CFData_uwb.pitch *= -1;
//CFData_uwb.yaw = CFData_vicon.yaw;
if(enableUWB && useUWB)
{
......
......@@ -35,8 +35,8 @@
using namespace d_fall_pps;
// Kalmanfilter Variables
#define KALMAN_M_VARIANCE 1 // was 1
#define KALMAN_P_VARIANCE 0.1
#define KALMAN_M_VARIANCE 0.01 // was 1
#define KALMAN_P_VARIANCE 0.0001
#define DISTANCE_ERROR_TRESHOLD 0.9
......@@ -67,7 +67,7 @@ static KalmanState xState;
static KalmanState yState;
static KalmanState zState;
//KalmanState kalmanDist[UWB_NUM_ANCHORS];
KalmanState kalmanDist[UWB_NUM_ANCHORS];
std::chrono::high_resolution_clock::time_point lastRPY;
......@@ -205,7 +205,7 @@ void d_fall_pps::cfDistancesCallback(const std_msgs::UInt32MultiArray &distances
data.y = arrXYZ[1];
data.z = arrXYZ[2];
data.roll = rollPitchYaw[0];
data.pitch = rollPitchYaw[1];
data.pitch = -1*rollPitchYaw[1];
data.yaw = rollPitchYaw[2];
data.occluded = false;
data.acquiringTime = 0; //???
......@@ -279,9 +279,9 @@ void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, doubl
//ROS_WARN("Kalman: (%f, %f, %f)", xState.x, yState.x, zState.x);
// Use this for unfiltered position
/*xyz[0] = result[0] / 10.0;
xyz[1] = result[1] / 10.0;
xyz[2] = result[2] / 10.0;*/
// xyz[0] = result[0] / 10.0;
// xyz[1] = result[1] / 10.0;
//xyz[2] = result[2] / 10.0;
publishLog(distances, dist);
}
......@@ -309,13 +309,14 @@ void d_fall_pps::publishLog(const std_msgs::UInt32MultiArray& distances, const d
void d_fall_pps::filterDistances(double* distances)
{
/*for (unsigned int i = 0; i < UWB_NUM_ANCHORS; i++)
for (unsigned int i = 0; i < UWB_NUM_ANCHORS; i++)
{
if(distances[i] != 0)
updateKalmanState(kalmanDist[i], distances[i]);
updateKalmanState(kalmanDist[i], distances[i]);
// if(distances[i] != 0)
// updateKalmanState(kalmanDist[i], distances[i]);
distances[i] = kalmanDist[i].x;
}*/
}
}
void d_fall_pps::initKalmanStates()
......@@ -337,17 +338,17 @@ void d_fall_pps::initKalmanStates()
// Init z state
// TODO Update
zState.x = 0;
zState.q = KALMAN_P_VARIANCE;
zState.r = KALMAN_M_VARIANCE;
zState.q = 5e-6; //KALMAN_P_VARIANCE;
zState.r = 5e-4; //KALMAN_M_VARIANCE;
zState.p = 0;
/*for(unsigned int i = 0; i < UWB_NUM_ANCHORS; i++)
for(unsigned int i = 0; i < UWB_NUM_ANCHORS; i++)
{
kalmanDist[i].x = 0;
kalmanDist[i].q = 0.005;
kalmanDist[i].r = 1;
kalmanDist[i].q = 8;
kalmanDist[i].r = 500;
kalmanDist[i].p = 0;
}*/
}
}
void d_fall_pps::updateKalmanState(KalmanState& state, double measurment)
......
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