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

Debug and tune

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