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 05bee7e4 authored by mastefan's avatar mastefan
Browse files

Kalman filter tested and working

parent 45714f1c
......@@ -462,7 +462,7 @@ std::vector<float> PMKF_Kinf_for_angles (2,0.0);
// float ms_kalman_Kinf2[2];
// #TODO: Check if getparam allows for float[] fetching
float ms_kalman_estimation_x[2] = {0,0};
float ms_kalman_estimation_x[2] = {1 , 1 };
float ms_kalman_estimation_y[2] = {0,0};
float ms_kalman_estimation_z[2] = {0,0};
......
......@@ -149,7 +149,21 @@ gainMatrixYawRatefromAngle : [ 0.00, 0.00, 2.30]
gainFeedforwardAnglefromVelocity: [-0.261799388, 0.261799388]
ms_kalman_Ahat1: [ 1.0 , 1.0 ]
ms_kalman_Ahat2: [ 1.0 , 1.0 ]
ms_kalman_Kinf1: [ 1.0 , 1.0 ]
ms_kalman_Kinf2: [ 1.0 , 1.0 ]
\ No newline at end of file
#ms_kalman_Ahat1: [ 5.87e-6 , 2.93e-8 ]
#ms_kalman_Ahat2: [ -0.9972 , 0.995 ]
#ms_kalman_Kinf1: [ 1 , 0 ]
#ms_kalman_Kinf2: [ 0.9972 , 0 ]
#ms_kalman_Ahat1: [ 0.6723, 0.0034]
#ms_kalman_Ahat2: [-12.9648, 0.9352]
#ms_kalman_Kinf1: [ 0.3277,0]
#ms_kalman_Kinf2: [ 12.9648 , 0 ]
ms_kalman_Ahat1: [ 0.6723, 0.0034]
ms_kalman_Ahat2: [-12.9648, 0.9352]
ms_kalman_Kinf1: [ 0.3277,0]
ms_kalman_Kinf2: [ 12.9648 , 0 ]
#PMKF_Ahat_row1_for_positions : [ 0.6723, 0.0034]
#PMKF_Ahat_row2_for_positions : [-12.9648, 0.9352]
#PMKF_Kinf_for_positions : [ 0.3277,12.9648]
\ No newline at end of file
......@@ -520,24 +520,29 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
ms_kalman_estimation_x[0] = ms_kalman_Ahat1[0] * ms_kalman_estimation_x[0] + ms_kalman_Ahat1[1] * ms_kalman_estimation_x[1]
+ ms_kalman_Kinf1[0] * current_MS_pos[0];
ms_kalman_estimation_x[1] = ms_kalman_Ahat2[0] * ms_kalman_estimation_x[0] + ms_kalman_Ahat2[1] * ms_kalman_estimation_x[1]
+ ms_kalman_Kinf2[0] * current_MS_pos[0];
// y position and velocity
ms_kalman_estimation_y[0] = ms_kalman_Ahat1[0] * ms_kalman_estimation_y[0] + ms_kalman_Ahat1[1] * ms_kalman_estimation_y[1]
+ ms_kalman_Kinf1[0] * current_MS_pos[1];
ms_kalman_estimation_y[1] = ms_kalman_Ahat2[0] * ms_kalman_estimation_y[0] + ms_kalman_Ahat2[1] * ms_kalman_estimation_y[1]
+ ms_kalman_Kinf2[0] * current_MS_pos[1];
// if(m_time_ticks % 200 == 0){
// ROS_INFO_STREAM("ms_kalman_estimation_x=" << ms_kalman_estimation_x[1]);
// }
// z position and velocity
ms_kalman_estimation_z[0] = ms_kalman_Ahat1[0] * ms_kalman_estimation_z[0] + ms_kalman_Ahat1[1] * ms_kalman_estimation_z[2]
+ ms_kalman_Kinf1[0] * current_MS_pos[2];
ms_kalman_estimation_z[1] = ms_kalman_Ahat2[0] * ms_kalman_estimation_z[0] + ms_kalman_Ahat2[1] * ms_kalman_estimation_z[2]
+ ms_kalman_Kinf2[0] * current_MS_pos[2];
// ms_kalman_estimation_x[0] = ms_kalman_Ahat1[0] * ms_kalman_estimation_x[0] + ms_kalman_Ahat1[1] * ms_kalman_estimation_x[1]
// + ms_kalman_Kinf1[0] * current_MS_pos[0];
// ms_kalman_estimation_x[1] = ms_kalman_Ahat2[0] * ms_kalman_estimation_x[0] + ms_kalman_Ahat2[1] * ms_kalman_estimation_x[1]
// + ms_kalman_Kinf2[0] * current_MS_pos[0];
// // y position and velocity
// ms_kalman_estimation_y[0] = ms_kalman_Ahat1[0] * ms_kalman_estimation_y[0] + ms_kalman_Ahat1[1] * ms_kalman_estimation_y[1]
// + ms_kalman_Kinf1[0] * current_MS_pos[1];
// ms_kalman_estimation_y[1] = ms_kalman_Ahat2[0] * ms_kalman_estimation_y[0] + ms_kalman_Ahat2[1] * ms_kalman_estimation_y[1]
// + ms_kalman_Kinf2[0] * current_MS_pos[1];
// // z position and velocity
// ms_kalman_estimation_z[0] = ms_kalman_Ahat1[0] * ms_kalman_estimation_z[0] + ms_kalman_Ahat1[1] * ms_kalman_estimation_z[2]
// + ms_kalman_Kinf1[0] * current_MS_pos[2];
// ms_kalman_estimation_z[1] = ms_kalman_Ahat2[0] * ms_kalman_estimation_z[0] + ms_kalman_Ahat2[1] * ms_kalman_estimation_z[2]
// + ms_kalman_Kinf2[0] * current_MS_pos[2];
if(request.otherCrazyflies[0].occluded){
......@@ -1755,13 +1760,13 @@ void calculateMSVelocity(Controller::Request &request){
current_MS_pos[1] = request.otherCrazyflies[0].y;
current_MS_pos[2] = request.otherCrazyflies[0].z;
if(calculate_ms_via_kalman){
if(!calculate_ms_via_kalman){
mothership_vel[0] = (current_MS_pos[0] - prev_MS_pos[0])*m_vicon_frequency;
mothership_vel[1] = (current_MS_pos[1] - prev_MS_pos[1])*m_vicon_frequency;
mothership_vel[2] = (current_MS_pos[2] - prev_MS_pos[2])*m_vicon_frequency;
}else{
//}else{
// stateInterialEstimate_viaPointMassKalmanFilter[0] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[0];
// stateInterialEstimate_viaPointMassKalmanFilter[3] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[0];
......@@ -1773,23 +1778,44 @@ void calculateMSVelocity(Controller::Request &request){
// stateInterialEstimate_viaPointMassKalmanFilter[5] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[2];
float temp_x[2];
float temp_y[2];
float temp_z[2];
for(int i = 0; i < 2; ++i)
{
temp_x[i] = ms_kalman_estimation_x[i];
temp_y[i] = ms_kalman_estimation_y[i];
temp_z[i] = ms_kalman_estimation_z[i];
}
// x position and velocity
ms_kalman_estimation_x[0] = ms_kalman_Ahat1[0] * ms_kalman_estimation_x[0] + ms_kalman_Ahat1[1] * ms_kalman_estimation_x[1]
+ ms_kalman_Kinf1[0] * current_MS_pos[0];
ms_kalman_estimation_x[1] = ms_kalman_Ahat2[0] * ms_kalman_estimation_x[0] + ms_kalman_Ahat2[1] * ms_kalman_estimation_x[1]
+ ms_kalman_Kinf2[0] * current_MS_pos[0];
ms_kalman_estimation_x[0] = ms_kalman_Ahat1[0] * temp_x[0] + ms_kalman_Ahat1[1] * temp_x[1] + ms_kalman_Kinf1[0] * current_MS_pos[0];
ms_kalman_estimation_x[1] = ms_kalman_Ahat2[0] * temp_x[0] + ms_kalman_Ahat2[1] * temp_x[1] + ms_kalman_Kinf2[0] * current_MS_pos[0];
// y position and velocity
ms_kalman_estimation_y[0] = ms_kalman_Ahat1[0] * ms_kalman_estimation_y[0] + ms_kalman_Ahat1[1] * ms_kalman_estimation_y[1]
+ ms_kalman_Kinf1[0] * current_MS_pos[1];
ms_kalman_estimation_y[1] = ms_kalman_Ahat2[0] * ms_kalman_estimation_y[0] + ms_kalman_Ahat2[1] * ms_kalman_estimation_y[1]
+ ms_kalman_Kinf2[0] * current_MS_pos[1];
ms_kalman_estimation_y[0] = ms_kalman_Ahat1[0] * temp_y[0] + ms_kalman_Ahat1[1] * temp_y[1] + ms_kalman_Kinf1[0] * current_MS_pos[1];
ms_kalman_estimation_y[1] = ms_kalman_Ahat2[0] * temp_y[0] + ms_kalman_Ahat2[1] * temp_y[1] + ms_kalman_Kinf2[0] * current_MS_pos[1];
// z position and velocity
ms_kalman_estimation_z[0] = ms_kalman_Ahat1[0] * ms_kalman_estimation_z[0] + ms_kalman_Ahat1[1] * ms_kalman_estimation_z[2]
+ ms_kalman_Kinf1[0] * current_MS_pos[2];
ms_kalman_estimation_z[1] = ms_kalman_Ahat2[0] * ms_kalman_estimation_z[0] + ms_kalman_Ahat2[1] * ms_kalman_estimation_z[2]
+ ms_kalman_Kinf2[0] * current_MS_pos[2];
ms_kalman_estimation_z[0] = ms_kalman_Ahat1[0] * temp_z[0] + ms_kalman_Ahat1[1] * temp_z[1] + ms_kalman_Kinf1[0] * current_MS_pos[2];
ms_kalman_estimation_z[1] = ms_kalman_Ahat2[0] * temp_z[0] + ms_kalman_Ahat2[1] * temp_z[1] + ms_kalman_Kinf2[0] * current_MS_pos[2];
if(m_time_ticks % 200 == 0){
ROS_INFO_STREAM("ms_kalman_estimation_x = " << ms_kalman_estimation_x[0]);
ROS_INFO_STREAM("ms_kalman_estimation_x dot = " << ms_kalman_estimation_x[1]);
//ROS_INFO_STREAM("ms_kalman_estimation_x=" << ms_kalman_Ahat1[0]);
//ROS_INFO_STREAM("ms_kalman_estimation_x=" << ms_kalman_Ahat1[1]);
ROS_INFO_STREAM("ms_kalman_estimation_x meas = " << current_MS_pos[0]);
}
}
......@@ -2252,8 +2278,8 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
debugMsg.value_6 = dronexSetpoint.y;
debugMsg.value_7 = dronexSetpoint.z;
debugMsg.value_8 = ms_kalman_estimation_x[1];
debugMsg.value_9 = ms_kalman_estimation_y[1];
debugMsg.value_8 = ms_kalman_estimation_x[1];
debugMsg.value_9 = ms_kalman_estimation_y[1];
debugMsg.value_10 = ms_kalman_estimation_z[1];
// ......................
// debugMsg.value_10 = your_variable_name;
......@@ -2952,7 +2978,7 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
getParameterFloatVector(nodeHandle_for_dronexController, "ms_kalman_Ahat1", ms_kalman_Ahat1, 2);
getParameterFloatVector(nodeHandle_for_dronexController, "ms_kalman_Ahat2", ms_kalman_Ahat2, 2);
getParameterFloatVector(nodeHandle_for_dronexController, "ms_kalman_Kinf1", ms_kalman_Kinf1, 2);
getParameterFloatVector(nodeHandle_for_dronexController, "ms_kalman_Kinf2", ms_kalman_Ahat2, 2);
getParameterFloatVector(nodeHandle_for_dronexController, "ms_kalman_Kinf2", ms_kalman_Kinf2, 2);
......
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