├── EKF_LIPRotellaEstimator.cpp ├── EKF_LIPRotellaEstimator.hpp ├── EKF_RotellaEstimator.cpp ├── EKF_RotellaEstimator.hpp └── README.md /EKF_LIPRotellaEstimator.cpp: -------------------------------------------------------------------------------- 1 | #include "EKF_LIPRotellaEstimator.hpp" 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | using namespace RigidBodyDynamics; 11 | 12 | EKF_LIPRotellaEstimator::EKF_LIPRotellaEstimator():Q_config(mercury::num_q), 13 | Q_config_dot(mercury::num_qdot), 14 | O_p_l(3), 15 | O_p_r(3), 16 | B_bf(3), 17 | B_bw(3), 18 | initial_foot_contact(false), 19 | lf_contact(false), 20 | rf_contact(false), 21 | prev_lf_contact(false), 22 | prev_rf_contact(false), 23 | f_imu(3), 24 | omega_imu(3) 25 | { 26 | 27 | lipm_height = 0.89; // Will be set by the initialization process; 28 | 29 | // Load Robot Model 30 | robot_model = new Model(); 31 | rbdl_check_api_version (RBDL_API_VERSION); 32 | if (!Addons::URDFReadFromFile (THIS_COM"/RobotSystems/Mercury/mercury.urdf", robot_model, false)) { 33 | std::cerr << "Error loading model ./mercury.urdf" << std::endl; 34 | abort(); 35 | } 36 | // -------------------- 37 | printf("[EKF Rotella Estimator] Successfully loaded URDF robot model."); 38 | 39 | // Initialize Global Positions 40 | O_r = dynacore::Vector::Zero(3); 41 | O_v = dynacore::Vector::Zero(3); 42 | 43 | Q_config.setZero(); 44 | Q_config_dot.setZero(); 45 | 46 | // Initialize Orientations 47 | O_q_B.w() = 1; 48 | O_q_B.x() = 0.0; O_q_B.y() = 0.0; O_q_B.z() = 0.0; 49 | Q_config[mercury::num_qdot] = 1.0; // Set orientation to identity. 50 | 51 | 52 | 53 | // Initialize remaining state vectors 54 | O_p_l.setZero(); 55 | O_p_r.setZero(); 56 | B_bf.setZero(); 57 | B_bw.setZero(); 58 | 59 | // Initialize inputs 60 | f_imu.setZero(); 61 | omega_imu.setZero(); 62 | 63 | count = 0; 64 | // Initialize EKF Variables 65 | local_gravity = 9.81; 66 | gravity_vec = dynacore::Vector::Zero(3); 67 | gravity_vec[2] = local_gravity; 68 | 69 | dt = mercury::servo_rate; 70 | dim_states = O_r.size() + O_v.size() + 4 + O_p_l.size() + O_p_r.size() + B_bf.size() + B_bw.size(); 71 | dim_rvq_states = O_r.size() + O_v.size() + 4; 72 | 73 | 74 | dim_error_states = O_r.size() + O_v.size() + 3 + O_p_l.size() + O_p_r.size() + B_bf.size() + B_bw.size(); 75 | dim_process_errors = f_imu.size() + omega_imu.size() + O_p_l.size() + O_p_r.size() + B_bf.size() + B_bw.size(); 76 | dim_obs = O_p_l.size() + O_p_r.size() + O_v.size() + O_v.size(); 77 | dim_inputs = f_imu.size() + omega_imu.size(); 78 | 79 | 80 | C_rot = dynacore::Matrix::Identity(3,3); 81 | F_c = dynacore::Matrix::Zero(dim_error_states, dim_error_states); 82 | L_c = dynacore::Matrix::Zero(dim_error_states, dim_process_errors); 83 | Q_c = dynacore::Matrix::Zero(dim_process_errors,dim_process_errors); 84 | 85 | F_k = dynacore::Matrix::Zero(dim_error_states, dim_error_states); 86 | H_k = dynacore::Matrix::Zero(dim_obs, dim_error_states); 87 | 88 | R_c = dynacore::Matrix::Zero(dim_obs, dim_obs); 89 | R_k = dynacore::Matrix::Zero(dim_obs, dim_obs); 90 | 91 | S_k = dynacore::Matrix::Zero(dim_obs, dim_obs); 92 | K_k = dynacore::Matrix::Zero(dim_error_states, dim_obs); 93 | 94 | P_prior = dynacore::Matrix::Zero(dim_error_states, dim_error_states); 95 | P_predicted = dynacore::Matrix::Zero(dim_error_states, dim_error_states); 96 | P_posterior = dynacore::Matrix::Zero(dim_error_states, dim_error_states); 97 | 98 | x_prior = dynacore::Vector::Zero(dim_states); 99 | x_predicted = dynacore::Vector::Zero(dim_states); 100 | x_posterior = dynacore::Vector::Zero(dim_states); 101 | x_prior[9] = 1.0; // Set Quaternion to identity. 102 | x_posterior[9] = 1.0; // Set Quaternion to identity. 103 | 104 | 105 | delta_x_prior = dynacore::Vector::Zero(dim_error_states); 106 | delta_x_posterior = dynacore::Vector::Zero(dim_error_states); 107 | 108 | delta_phi.setZero(); 109 | 110 | delta_y = dynacore::Vector::Zero(dim_obs); 111 | 112 | z_lfoot_pos_B = dynacore::Vector::Zero(O_p_l.size()); 113 | z_rfoot_pos_B = dynacore::Vector::Zero(O_p_r.size()); 114 | 115 | p_l_B = dynacore::Vector::Zero(O_p_l.size()); 116 | p_r_B = dynacore::Vector::Zero(O_p_r.size()); 117 | 118 | body_vel_kinematics = dynacore::Vector::Zero(O_v.size()); 119 | body_com_vel_kinematics = dynacore::Vector::Zero(O_v.size()); 120 | body_imu_vel = dynacore::Vector::Zero(O_v.size()); 121 | 122 | y_vec = dynacore::Vector::Zero(dim_obs); 123 | 124 | // Initialize Covariance parameters 125 | // Values are from reference paper. Need to be changed to known IMU parameters 126 | ww_intensity = 0.000523; // rad/s/sqrt(Hz) // angular velocity process noise intensity 127 | 128 | wp_intensity_default = 0.001;//0.001; // m/sqrt(Hz) // default foot location noise intensity 129 | wp_intensity_unknown = 1000.0; // m/sqrt(Hz) // noise intensity when there is no foot contact 130 | // Simulation params 131 | wf_intensity = 0.001;//0.00078; // m/(s^2)/sqrt(Hz) // imu process noise intensity 132 | wbf_intensity = 10.0; // m/(s^3)/sqrt(Hz) // imu bias intensity 133 | 134 | // Real sensor params 135 | // wf_intensity = 0.01;//0.00078; // m/(s^2)/sqrt(Hz) // imu process noise intensity 136 | // wbf_intensity = 10.0; // m/(s^3)/sqrt(Hz) // imu bias intensity 137 | wbw_intensity = 0.000618; // rad/(s^2)/sqrt(Hz) // ang vel bias intensity 138 | 139 | n_p = 0.01; // foot measurement noise intensity. 140 | 141 | //n_v_default = 0.01; // default noise intensity of body velocity measurement 142 | n_v_default = 10.0; // default noise intensity of body velocity measurement 143 | n_v_unknown = 1000; // unknown noise intensity from body velocity measurement 144 | 145 | n_com_default = 0.01; // default noise intensity of body velocity measurement 146 | n_com_unknown = 1000; // unknown noise intensity from body velocity measurement 147 | 148 | _SetupParameters(); 149 | wp_l_intensity = wp_intensity_unknown; // m/sqrt(Hz) // left foot location noise intensity 150 | wp_r_intensity = wp_intensity_unknown; // m/sqrt(Hz) // right foot location noise intensity 151 | 152 | n_v = n_v_unknown; // Body velocity from kinematics measurement noise intensity 153 | n_com = n_com_default; // Body velocity from kinematics measurement noise intensity 154 | 155 | // Initialize Orientation Calibration Filters 156 | // Bias Filter 157 | bias_lp_frequency_cutoff = 2.0*3.1415*1.0; // 1Hz // (2*pi*frequency) rads/s 158 | x_bias_low_pass_filter = new digital_lp_filter(bias_lp_frequency_cutoff, mercury::servo_rate); 159 | y_bias_low_pass_filter = new digital_lp_filter(bias_lp_frequency_cutoff, mercury::servo_rate); 160 | z_bias_low_pass_filter = new digital_lp_filter(bias_lp_frequency_cutoff, mercury::servo_rate); 161 | 162 | // Initialize body orientation to identity. 163 | dynacore::Vect3 rpy_init; rpy_init.setZero(); 164 | dynacore::convert(rpy_init, Oq_B_init); 165 | OR_B_init = Oq_B_init.toRotationMatrix(); 166 | 167 | // Initialize other calibration parameters 168 | gravity_mag = 9.81; // m/s^2; 169 | theta_x = 0.0; 170 | theta_y = 0.0; 171 | 172 | x_acc_bias = 0.0; 173 | y_acc_bias = 0.0; 174 | z_acc_bias = 0.0; 175 | 176 | g_B.setZero(); 177 | g_B_local_vec.setZero(); 178 | 179 | // Register data for logging 180 | 181 | DataManager::GetDataManager()->RegisterData(&O_r, DYN_VEC, "ekf_o_r", 3); 182 | DataManager::GetDataManager()->RegisterData(&O_v, DYN_VEC, "ekf_o_v", 3); 183 | DataManager::GetDataManager()->RegisterData(&O_q_B, QUATERNION, "ekf_o_q_b", 4); 184 | DataManager::GetDataManager()->RegisterData(&f_imu, DYN_VEC, "ekf_f_imu", 3); 185 | DataManager::GetDataManager()->RegisterData(&omega_imu, DYN_VEC, "ekf_omega_imu", 3); 186 | DataManager::GetDataManager()->RegisterData(&B_bf, DYN_VEC, "ekf_B_bf", 3); 187 | DataManager::GetDataManager()->RegisterData(&B_bw, DYN_VEC, "ekf_B_bw", 3); 188 | DataManager::GetDataManager()->RegisterData(&y_vec, DYN_VEC, "ekf_measured_diff", 6); 189 | 190 | DataManager::GetDataManager()->RegisterData(&z_lfoot_pos_B, DYN_VEC, "ekf_z_l_foot_B", 3); 191 | DataManager::GetDataManager()->RegisterData(&z_rfoot_pos_B, DYN_VEC, "ekf_z_r_foot_B", 3); 192 | DataManager::GetDataManager()->RegisterData(&p_l_B, DYN_VEC, "ekf_p_left_B", 3); 193 | DataManager::GetDataManager()->RegisterData(&p_r_B, DYN_VEC, "ekf_p_right_B", 3); 194 | 195 | DataManager::GetDataManager()->RegisterData(&O_p_l, DYN_VEC, "ekf_p_left_O", 3); 196 | DataManager::GetDataManager()->RegisterData(&O_p_r, DYN_VEC, "ekf_p_right_O", 3); 197 | 198 | 199 | DataManager::GetDataManager()->RegisterData(&body_vel_kinematics, DYN_VEC, "ekf_body_vel_kin", 3); 200 | DataManager::GetDataManager()->RegisterData(&body_com_vel_kinematics, DYN_VEC, "ekf_com_vel_kin", 3); 201 | 202 | DataManager::GetDataManager()->RegisterData(&body_imu_vel, DYN_VEC, "ekf_imu_vel", 3); 203 | 204 | } 205 | 206 | void EKF_LIPRotellaEstimator::_SetupParameters(){ 207 | ParamHandler handler(MercuryConfigPath"ESTIMATOR_ekf_lipm.yaml"); 208 | handler.getValue("n_p", n_p); 209 | handler.getValue("ww_intensity", ww_intensity); 210 | handler.getValue("wp_intensity_default", wp_intensity_default); 211 | handler.getValue("wp_intensity_unknown", wp_intensity_unknown); 212 | // Simulation params 213 | handler.getValue("wf_intensity", wf_intensity); 214 | handler.getValue("wbf_intensity", wbf_intensity); 215 | 216 | // Real sensor params 217 | handler.getValue("wbw_intensity", wbw_intensity); 218 | handler.getValue("n_v_default", n_v_default); 219 | handler.getValue("n_v_unknown", n_v_unknown); 220 | 221 | printf("2\n"); 222 | handler.getValue("n_com_default", n_com_default); 223 | handler.getValue("n_com_unknown", n_com_unknown); 224 | printf("2\n"); 225 | } 226 | 227 | 228 | EKF_LIPRotellaEstimator::~EKF_LIPRotellaEstimator(){ 229 | delete robot_model; 230 | } 231 | 232 | void EKF_LIPRotellaEstimator::EstimatorInitializationWithCOMHeight(const dynacore::Quaternion & initial_global_orientation, 233 | const std::vector & initial_imu_acc, 234 | const std::vector & initial_imu_ang_vel, 235 | const double com_height){ 236 | 237 | // Initialize linear inverted pendulum height 238 | lipm_height = com_height; 239 | EstimatorInitialization(initial_global_orientation, initial_imu_acc, initial_imu_ang_vel); 240 | 241 | } 242 | 243 | void EKF_LIPRotellaEstimator::EstimatorInitialization(const dynacore::Quaternion & initial_global_orientation, 244 | const std::vector & initial_imu_acc, 245 | const std::vector & initial_imu_ang_vel){ 246 | 247 | 248 | // Initialize IMU values 249 | for(size_t i = 0; i < 3; i++){ 250 | f_imu[i] = -initial_imu_acc[i]; 251 | omega_imu[i] = initial_imu_ang_vel[i]; 252 | } 253 | 254 | // Initialize Covariance matrix prior 255 | getMatrix_L_c(O_q_B, L_c); 256 | getMatrix_Q(Q_c); 257 | P_prior = L_c*Q_c*L_c.transpose();// L_c*Q_c; 258 | 259 | // Initialize Global Orientation 260 | //O_q_B = initial_global_orientation; 261 | 262 | // // Initialize global orientation 263 | calibrateOrientationFromGravity(); 264 | 265 | x_prior[O_r.size()+O_v.size()] = O_q_B.x(); 266 | x_prior[O_r.size()+O_v.size()+1] = O_q_B.y(); 267 | x_prior[O_r.size()+O_v.size()+2] = O_q_B.z(); 268 | x_prior[O_r.size()+O_v.size()+3] = O_q_B.w(); 269 | 270 | } 271 | 272 | void EKF_LIPRotellaEstimator::calibrateOrientationFromGravity(){ 273 | //printf("[EKF Rotella Estimator] Initializing Orientation from IMU acceleration data. The robot should not be moving \n"); 274 | 275 | // Update bias estimate 276 | x_bias_low_pass_filter->input(f_imu[0]); 277 | y_bias_low_pass_filter->input(f_imu[1]); 278 | z_bias_low_pass_filter->input(f_imu[2]); 279 | 280 | x_acc_bias = x_bias_low_pass_filter->output(); 281 | y_acc_bias = y_bias_low_pass_filter->output(); 282 | z_acc_bias = z_bias_low_pass_filter->output(); 283 | 284 | // Finds The orientation of the body with respect to the fixed frame O ((^OR_B ). 285 | // This assumes that the IMU can only sense gravity as the acceleration. 286 | // 287 | // The algorithm attempts to solve ^OR_B * f_b = f_o, 288 | // where f_b is the acceleration of gravity in the body frame. 289 | // f_o is the acceleration of gravity in the fixed frame 290 | // In addition to ^OR_B acting as a change of frame formula, 291 | // note that ^OR_B will also equal to the orientation of the body w.r.t to the fixed frame. 292 | 293 | // We will rotate f_b using an extrinsic rotation with global R_x (roll) and R_y (pitch) rotations 294 | // Thus, we will perform ^OR_y ^OR_x f_b = f_o. 295 | 296 | // Finally, note that ^OR_b = ^OR_y ^OR_x. 297 | // The resulting orientation will have the xhat component of ^OR_b (ie: ^OR_b.col(0)) to be always planar 298 | // with the inertial x-z plane. 299 | // 300 | // It is best to visualize the extrinsic rotation on paper for any given extrinsic roll then pitch operations 301 | 302 | g_B.setZero(); 303 | g_B[0] = x_acc_bias; 304 | g_B[1] = y_acc_bias; 305 | g_B[2] = z_acc_bias; // We expect a negative number if gravity is pointing opposite of the IMU zhat direction 306 | 307 | gravity_mag = g_B.norm(); 308 | g_B /= gravity_mag; 309 | 310 | // Prepare to rotate gravity vector 311 | g_B_local_vec[0] = g_B[0]; g_B_local_vec[1] = g_B[1]; g_B_local_vec[2] = g_B[2]; 312 | 313 | //--------------------------------------------------------------------------- 314 | // Use Rx to rotate the roll and align gravity vector - 315 | // Compute Roll to rotate 316 | // theta_x = atan(g_B[1]/g_B[2]); 317 | // double roll_val = theta_x; 318 | 319 | // The following method can handle any initial vector due to gravity 320 | theta_x = atan2(g_B_local_vec[2], g_B_local_vec[1]); // Returns angle \in [-pi, pi] between z and y projected vectors. 321 | double roll_val = (-M_PI/2.0 - theta_x); // (-pi/2 - theta_x) 322 | roll_value_comp = roll_val; 323 | 324 | //dynacore::convert(0.0, 0.0, roll_val, q_world_Rx); 325 | // Create Roll Quaternion 326 | q_world_Rx.w() = cos(roll_val/2.);; 327 | q_world_Rx.x() = sin(roll_val/2.); 328 | q_world_Rx.y() = 0; 329 | q_world_Rx.z() = 0; 330 | 331 | //Rotate gravity vector to align the yhat directions 332 | dynacore::Matrix Rx = q_world_Rx.normalized().toRotationMatrix(); 333 | g_B_local_vec = Rx*g_B_local_vec; 334 | 335 | // Use Ry to rotate pitch and align gravity vector --------------------------- 336 | // Compute Pitch to rotate 337 | // theta_y = atan(g_B_local.x()/g_B_local.z()); 338 | // double pitch_val = -theta_y; 339 | 340 | // The following method can handle any initial vector due to gravity 341 | theta_y = atan2(g_B_local_vec[2], g_B_local_vec[0]); // Returns angle \in [-pi, pi] between z and x projected vectors. 342 | double pitch_val = -((-M_PI/2.0) - theta_y); // This is actually -(-pi/2 - theta_y) 343 | pitch_value_comp = pitch_val; 344 | 345 | //dynacore::convert(0.0, pitch_val, 0.0, q_world_Ry); 346 | // Create Pitch Quaternion 347 | q_world_Ry.w() = cos(pitch_val/2.); 348 | q_world_Ry.x() = 0; 349 | q_world_Ry.y() = sin(pitch_val/2.); 350 | q_world_Ry.z() = 0; 351 | 352 | // Rotate gravity vector to align the xhat directions 353 | dynacore::Matrix Ry = q_world_Ry.normalized().toRotationMatrix(); 354 | g_B_local_vec = Ry*g_B_local_vec; 355 | 356 | // Obtain initial body orientation w.r.t fixed frame. 357 | //Oq_B = q_y * q_x * q_b 358 | Oq_B_init = dynacore::QuatMultiply(q_world_Ry, q_world_Rx); 359 | // Set rotation matrix 360 | OR_B_init = Oq_B_init.normalized().toRotationMatrix(); 361 | 362 | // Initialize orientation 363 | O_q_B = Oq_B_init; 364 | 365 | // dynacore::pretty_print(g_B, std::cout, "gravity_dir"); 366 | // printf(" gravity_mag = %0.4f \n", gravity_mag); 367 | // printf(" theta_x = %0.4f \n", theta_x); 368 | // printf(" theta_y = %0.4f \n", theta_y); 369 | // printf(" roll_value_comp = %0.4f \n", roll_value_comp); 370 | // printf(" pitch_value_comp = %0.4f \n", pitch_value_comp); 371 | // dynacore::pretty_print(g_B_local_vec, std::cout, "rotated gravity vec"); 372 | 373 | // dynacore::pretty_print(Oq_B_init, std::cout, "Initial body orientation w.r.t fixed frame: "); 374 | // dynacore::pretty_print(OR_B_init, std::cout, "OR_B_init: "); 375 | // dynacore::pretty_print(O_q_B, std::cout, "Body orientation w.r.t fixed frame: "); 376 | // printf("\n"); 377 | 378 | } 379 | 380 | 381 | void EKF_LIPRotellaEstimator::computeNewFootLocations(const int foot_link_id){ 382 | // Find Foot body id 383 | unsigned int bodyid; 384 | switch(foot_link_id){ 385 | case mercury_link::leftFoot: 386 | bodyid = robot_model->GetBodyId("lfoot"); 387 | break; 388 | case mercury_link::rightFoot: 389 | bodyid = robot_model->GetBodyId("rfoot"); 390 | break; 391 | default: 392 | printf("[EKF_LIPRotellaEstimator] Not a valid foot link id\n"); 393 | exit(0); 394 | } 395 | 396 | // Set parameters up 397 | dynacore::Vect3 Local_CoM = robot_model->mFixedBodies[ 398 | bodyid - robot_model->fixed_body_discriminator].mCenterOfMass; 399 | 400 | Q_config.head(O_r.size()) = O_r; 401 | Q_config[O_r.size()] = O_q_B.x(); 402 | Q_config[O_r.size()+1] = O_q_B.y(); 403 | Q_config[O_r.size()+2] = O_q_B.z(); 404 | Q_config[mercury::num_qdot] = O_q_B.w(); 405 | 406 | // Update new foot locations 407 | if (foot_link_id == mercury_link::leftFoot){ 408 | O_p_l = CalcBodyToBaseCoordinates(*robot_model, Q_config, bodyid, Local_CoM, true); 409 | x_prior.segment(dim_rvq_states, O_p_l.size()) = O_p_l; 410 | } 411 | if (foot_link_id == mercury_link::rightFoot){ 412 | O_p_r = CalcBodyToBaseCoordinates(*robot_model, Q_config, bodyid, Local_CoM, true); 413 | x_prior.segment(dim_rvq_states + O_p_l.size(), O_p_r.size()) = O_p_r; 414 | } 415 | 416 | 417 | } 418 | 419 | void EKF_LIPRotellaEstimator::resetFilter(){ 420 | // Initialize Covariance matrix prior 421 | getMatrix_L_c(O_q_B, L_c); 422 | getMatrix_Q(Q_c); 423 | P_prior = L_c*Q_c*L_c.transpose(); 424 | 425 | // // Initialize state vector except for the orientation 426 | x_prior.head(O_r.size() + O_v.size()) = dynacore::Vector::Zero(O_r.size() + O_v.size()); 427 | x_prior.tail(O_p_l.size() + O_p_r.size() + B_bf.size() + B_bw.size()) = dynacore::Vector::Zero(O_p_l.size() + O_p_r.size() + B_bf.size() + B_bw.size()); 428 | 429 | setStateVariablesToPrior(); 430 | // x_prior = dynacore::Vector::Zero(dim_states); 431 | // x_prior[9] = 1.0; 432 | body_vel_kinematics = dynacore::Vector::Zero(3); 433 | body_com_vel_kinematics = dynacore::Vector::Zero(3); 434 | body_imu_vel = dynacore::Vector::Zero(3); 435 | 436 | computeNewFootLocations(mercury_link::leftFoot); // Update Left foot location 437 | computeNewFootLocations(mercury_link::rightFoot); // Update Right foot location 438 | 439 | 440 | } 441 | 442 | void EKF_LIPRotellaEstimator::setSensorData(const std::vector & acc, 443 | const std::vector & acc_inc, 444 | const std::vector & ang_vel, 445 | const bool left_foot_contact, 446 | const bool right_foot_contact, 447 | const dynacore::Vector & joint_values, 448 | const dynacore::Vector & joint_velocity_values){ 449 | 450 | 451 | Q_config_dot.segment(mercury::num_virtual, mercury::num_act_joint) = joint_velocity_values; 452 | setSensorData(acc, acc_inc, ang_vel, left_foot_contact, right_foot_contact, joint_values); 453 | 454 | } 455 | 456 | void EKF_LIPRotellaEstimator::setSensorData(const std::vector & acc, 457 | const std::vector & acc_inc, 458 | const std::vector & ang_vel, 459 | const bool & left_foot_contact, 460 | const bool & right_foot_contact, 461 | const dynacore::Vector & joint_values){ 462 | 463 | 464 | for(size_t i = 0; i < 3; i++){ 465 | f_imu[i] = -acc[i]; 466 | omega_imu[i] = ang_vel[i]; 467 | } 468 | //f_imu[i] = acc[i]; 469 | //f_imu[2] = -f_imu[2]; 470 | 471 | lf_contact = left_foot_contact; 472 | rf_contact = right_foot_contact; 473 | Q_config.segment(mercury::num_virtual, mercury::num_act_joint) = joint_values; 474 | 475 | // Note: the first time contact is activated, the estimator is reset. 476 | if ( (!initial_foot_contact) && ((lf_contact) || (rf_contact)) ) { 477 | printf("\n"); 478 | printf("[EKF_LIPRotellaEstimator] initial contact triggered. EKF filter will be reset. \n"); 479 | printf("\n"); 480 | initial_foot_contact = true; 481 | resetFilter(); 482 | } 483 | 484 | 485 | // Handle changes in foot contact 486 | handleFootContacts(); 487 | 488 | // Perform filter calculations given sensor data 489 | doFilterCalculations(); 490 | 491 | //Print Statements 492 | if (count % 100 == 0){ 493 | showPrintOutStatements(); 494 | count = 0; 495 | } 496 | count++; 497 | 498 | // Update foot contact booleans 499 | prev_lf_contact = lf_contact; 500 | prev_rf_contact = rf_contact; 501 | 502 | } 503 | 504 | void EKF_LIPRotellaEstimator::handleFootContacts(){ 505 | // if the foot location is lost, set wp to a high number 506 | if (lf_contact){ 507 | wp_l_intensity = wp_intensity_default; 508 | }else{ 509 | wp_l_intensity = wp_intensity_unknown; 510 | } 511 | // if the foot location is lost, set wp to a high number 512 | if (rf_contact){ 513 | wp_r_intensity = wp_intensity_default; 514 | }else{ 515 | wp_r_intensity = wp_intensity_unknown; 516 | } 517 | 518 | // Set n_v to a high number if neither foot contact is available 519 | if (lf_contact || rf_contact){ 520 | n_v = n_v_default; 521 | n_com = n_com_default; 522 | }else{ 523 | n_v = n_v_unknown; 524 | n_com = n_com_unknown; 525 | } 526 | 527 | // Handle new contact detections 528 | // Check if a new foot location will be used for estimation 529 | if ((prev_lf_contact == false) && (lf_contact == true)){ 530 | // printf("\n New Left foot contact\n"); 531 | computeNewFootLocations(mercury_link::leftFoot); // Update Left foot location 532 | // Update Prior? 533 | //P_prior.block(9,9,3,3) = wp_intensity_default*dynacore::Matrix::Identity(3,3); 534 | 535 | } 536 | if ((prev_rf_contact == false) && (rf_contact == true)){ 537 | // printf("\n New Right foot contact\n"); 538 | computeNewFootLocations(mercury_link::rightFoot); // Update right foot location 539 | // Update Prior? 540 | //P_prior.block(12,12,3,3) = wp_intensity_default*dynacore::Matrix::Identity(3,3); 541 | } 542 | 543 | // Handle loss of contact 544 | // Update Covariance when a loss in contact is detected 545 | if ((prev_lf_contact == true) && (lf_contact == false)){ 546 | //P_prior.block(9,9,3,3) = wp_intensity_unknown*dynacore::Matrix::Identity(3,3); 547 | } 548 | if ((prev_rf_contact == true) && (rf_contact == false)){ 549 | //P_prior.block(12,12,3,3) = wp_intensity_unknown*dynacore::Matrix::Identity(3,3); 550 | } 551 | } 552 | 553 | dynacore::Matrix EKF_LIPRotellaEstimator::getSkewSymmetricMatrix(dynacore::Vector & vec_in){ 554 | dynacore::Matrix ssm = dynacore::Matrix::Zero(3,3); 555 | ssm(0,1) = -vec_in[2]; ssm(0,2) = vec_in[1]; 556 | ssm(1,0) = vec_in[2]; ssm(1,2) = -vec_in[0]; 557 | ssm(2,0) = -vec_in[1]; ssm(2,1) = vec_in[0]; 558 | return ssm; 559 | } 560 | 561 | void EKF_LIPRotellaEstimator::setStateVariablesToPrior(){ 562 | // Get prior values: 563 | O_r = x_prior.head(O_r.size()); 564 | O_v = x_prior.segment(O_r.size(), O_v.size()); 565 | O_q_B.x() = x_prior[O_r.size() + O_v.size()]; 566 | O_q_B.y() = x_prior[O_r.size() + O_v.size() + 1]; 567 | O_q_B.z() = x_prior[O_r.size() + O_v.size() + 2]; 568 | O_q_B.w() = x_prior[O_r.size() + O_v.size() + 3]; 569 | 570 | C_rot = O_q_B.toRotationMatrix(); 571 | 572 | O_p_l = x_prior.segment(dim_rvq_states, O_p_l.size()); 573 | O_p_r = x_prior.segment(dim_rvq_states + O_p_l.size(), O_p_r.size()); 574 | B_bf = x_prior.segment(dim_rvq_states + O_p_l.size() + O_p_r.size(), B_bf.size()); 575 | B_bw = x_prior.segment(dim_rvq_states + O_p_l.size() + O_p_r.size() + B_bw.size(), B_bw.size()); 576 | 577 | B_bf = x_prior.segment(dim_rvq_states + O_p_l.size() + O_p_r.size(), B_bf.size()); 578 | B_bw = x_prior.segment(dim_rvq_states + O_p_l.size() + O_p_r.size() + B_bw.size(), B_bw.size()); 579 | 580 | } 581 | 582 | void EKF_LIPRotellaEstimator::setStateVariablesToPredicted(){ 583 | O_r = x_predicted.head(O_r.size()); 584 | O_v = x_predicted.segment(O_r.size(), O_v.size()); 585 | O_q_B.x() = x_predicted[O_r.size() + O_v.size()]; 586 | O_q_B.y() = x_predicted[O_r.size() + O_v.size() + 1]; 587 | O_q_B.z() = x_predicted[O_r.size() + O_v.size() + 2]; 588 | O_q_B.w() = x_predicted[O_r.size() + O_v.size() + 3]; 589 | 590 | C_rot = O_q_B.toRotationMatrix(); 591 | 592 | O_p_l = x_predicted.segment(dim_rvq_states, O_p_l.size()); 593 | O_p_r = x_predicted.segment(dim_rvq_states + O_p_l.size(), O_p_r.size()); 594 | B_bf = x_predicted.segment(dim_rvq_states + O_p_l.size() + O_p_r.size(), B_bf.size()); 595 | B_bw = x_predicted.segment(dim_rvq_states + O_p_l.size() + O_p_r.size() + B_bf.size(), B_bw.size()); 596 | } 597 | 598 | void EKF_LIPRotellaEstimator::statePredictionStep(){ 599 | // Prepare state variables 600 | setStateVariablesToPrior(); 601 | 602 | // Prepare filter input: 603 | //f_imu_input = C_rot * f_imu; 604 | f_imu_input = f_imu - B_bf; 605 | // omega_imu_input = omega_imu - B_bw; 606 | omega_imu_input = omega_imu; 607 | 608 | 609 | // Prepare rotation values 610 | dynacore::Quaternion B_q_omega; 611 | dynacore::Vect3 vec3_omega_input; vec3_omega_input.setZero(); 612 | for(size_t i = 0; i < 3; i++){ 613 | vec3_omega_input[i] = omega_imu_input[i]; 614 | } 615 | dynacore::convert(vec3_omega_input*dt, B_q_omega); 616 | 617 | // Perform Discrete State prediction step; 618 | // predict position and velocity 619 | 620 | double omega(local_gravity/lipm_height); 621 | 622 | if (lf_contact){ 623 | // Use IP With the left foot swing 624 | x_predicted.head(2) = O_r.head(2) + O_v.head(2)*dt + 0.5*dt*dt*omega*(O_r.head(2) - O_p_l.head(2)); 625 | x_predicted[2] = O_r[2] + O_v[2]*dt; 626 | 627 | x_predicted.segment(3,2) = O_v + dt*omega*(O_r.head(2) - O_p_l.head(2)); 628 | x_predicted[5] = O_v[5]; 629 | 630 | }else if(rf_contact){ 631 | // Use IP With the right foot swing 632 | x_predicted.head(2) = O_r.head(2) + O_v.head(2)*dt + 0.5*dt*dt*omega*(O_r.head(2) - O_p_r.head(2)); 633 | x_predicted[2] = O_r[2] + O_v[2]*dt; 634 | 635 | x_predicted.segment(3,2) = O_v + dt*omega*(O_r.head(2) - O_p_r.head(2)); 636 | x_predicted[5] = O_v[5]; 637 | }else{ 638 | x_predicted.segment(0, O_r.size()) = O_r + O_v*dt + 0.5*dt*dt*(C_rot.transpose()*f_imu_input + gravity_vec); 639 | x_predicted.segment(O_r.size(), O_v.size()) = O_v + dt*(C_rot.transpose()*f_imu_input + gravity_vec); 640 | } 641 | 642 | //x_predicted.segment(0, O_r.size()) = O_r + O_v*dt + 0.5*dt*dt*(C_rot.transpose()*f_imu_input + gravity_vec); 643 | //x_predicted.segment(O_r.size(), O_v.size()) = O_v + dt*(C_rot.transpose()*f_imu_input + gravity_vec); 644 | 645 | // predict orientation 646 | dynacore::Quaternion q_predicted = dynacore::QuatMultiply(O_q_B, B_q_omega); 647 | // dynacore::Quaternion q_predicted = dynacore::QuatMultiply(B_q_omega, O_q_B); 648 | 649 | x_predicted[O_r.size()+O_v.size()] = q_predicted.x(); 650 | x_predicted[O_r.size()+O_v.size()+1] = q_predicted.y(); 651 | x_predicted[O_r.size()+O_v.size()+2] = q_predicted.z(); 652 | x_predicted[O_r.size()+O_v.size()+3] = q_predicted.w(); 653 | 654 | // predict foot position and biases 655 | x_predicted.segment(dim_rvq_states, dim_states - dim_rvq_states) = x_prior.segment(dim_rvq_states, dim_states - dim_rvq_states); 656 | 657 | // Set variables to predicted 658 | setStateVariablesToPredicted(); 659 | } 660 | 661 | void EKF_LIPRotellaEstimator::getMatrix_L_c(const dynacore::Quaternion & q_in, dynacore::Matrix & L_c_mat){ 662 | dynacore::Matrix C_mat = q_in.toRotationMatrix(); 663 | L_c_mat = dynacore::Matrix::Zero(dim_error_states, dim_process_errors); 664 | 665 | // L_c_mat.block(O_r.size(), 0, 3, 3) = -C_mat.transpose(); 666 | // L_c_mat.block(O_r.size() + O_v.size(), f_imu.size(), 3, 3) = -dynacore::Matrix::Identity(3,3); 667 | // L_c_mat.block(O_r.size() + O_v.size() + 3, f_imu.size() + omega_imu.size(), 3,3) = C_mat.transpose(); 668 | // L_c_mat.block(O_r.size() + O_v.size() + 3 + O_p_l.size(), f_imu.size() + omega_imu.size() + O_p_l.size(), 3, 3) = C_mat.transpose(); 669 | // L_c_mat.block(O_r.size() + O_v.size() + 3 + O_p_l.size() + O_p_r.size(), f_imu.size() + omega_imu.size() + O_p_l.size() + O_p_r.size(), 3, 3) = dynacore::Matrix::Identity(3,3); 670 | // L_c_mat.block(O_r.size() + O_v.size() + 3 + O_p_l.size() + O_p_r.size() + B_bw.size(), 671 | // f_imu.size() + omega_imu.size() + O_p_l.size() + O_p_r.size() + B_bw.size(), 3, 3) = dynacore::Matrix::Identity(3,3); 672 | 673 | double omega(local_gravity/lipm_height); 674 | dynacore::Matrix hg_mat = dynacore::Matrix::Identity(3,3); 675 | hg_mat(0,0) = omega; 676 | hg_mat(1,1) = omega; 677 | if(lf_contact){ 678 | L_c_mat.block(3, 0, 3, 3) = hg_mat; 679 | L_c_mat.block(3, 9, 3, 3) = hg_mat; 680 | }else if(rf_contact){ 681 | L_c_mat.block(3, 0, 3, 3) = hg_mat; 682 | L_c_mat.block(3, 12, 3, 3) = hg_mat; 683 | }else{ 684 | L_c_mat.block(3, 0, 3, 3) = -C_mat.transpose(); 685 | } 686 | 687 | //L_c_mat.block(3, 0, 3, 3) = -C_mat.transpose(); 688 | L_c_mat.block(6, 3, 3, 3) = -dynacore::Matrix::Identity(3,3); 689 | L_c_mat.block(9, 6, 3,3) = C_mat.transpose(); 690 | L_c_mat.block(12, 9, 3, 3) = C_mat.transpose(); 691 | L_c_mat.block(15, 12, 3, 3) = dynacore::Matrix::Identity(3,3); 692 | L_c_mat.block(18, 15, 3, 3) = dynacore::Matrix::Identity(3,3); 693 | 694 | 695 | 696 | } 697 | 698 | void EKF_LIPRotellaEstimator::getMatrix_Q(dynacore::Matrix & Q_mat){ 699 | Q_mat = dynacore::Matrix::Zero(dim_process_errors, dim_process_errors); 700 | Q_mat.block(0, 0, 3, 3) = wf_intensity*dynacore::Matrix::Identity(3,3); 701 | Q_mat.block(3, 3, 3, 3) = ww_intensity*dynacore::Matrix::Identity(3,3); 702 | Q_mat.block(6, 6, 3, 3) = wp_l_intensity*dynacore::Matrix::Identity(3,3); 703 | Q_mat.block(9, 9, 3, 3) = wp_r_intensity*dynacore::Matrix::Identity(3,3); 704 | Q_mat.block(12, 12, 3, 3) = wbf_intensity*dynacore::Matrix::Identity(3,3); 705 | Q_mat.block(15, 15, 3, 3) = wbw_intensity*dynacore::Matrix::Identity(3,3); 706 | 707 | } 708 | 709 | void EKF_LIPRotellaEstimator::covariancePredictionStep(){ 710 | // Prepare Covariance Prediction Step 711 | // Construct linearized error dynamics matrix, F_c 712 | F_c = dynacore::Matrix::Zero(dim_error_states, dim_error_states); 713 | // F_c.block(0, O_r.size(), 3, 3) = dynacore::Matrix::Identity(3, 3); 714 | // F_c.block(O_r.size(), O_r.size() + O_v.size(), 3, 3) = -C_rot.transpose()*getSkewSymmetricMatrix(f_imu_input); 715 | // F_c.block(O_r.size(), dim_rvq_states + O_p_l.size() + O_p_r.size(), 3, 3) = -C_rot.transpose(); 716 | // F_c.block(O_r.size() + O_v.size(), O_r.size() + O_v.size(), 3, 3) = -getSkewSymmetricMatrix(omega_imu_input); 717 | // F_c.block(O_r.size() + O_v.size(), dim_error_states - B_bw.size(), B_bw.size(), B_bw.size()) = -dynacore::Matrix::Identity(3,3); 718 | 719 | F_c.block(0, 3, 3, 3) = dynacore::Matrix::Identity(3, 3); 720 | 721 | if(lf_contact){ 722 | F_c.block(3, 0, 2, 2) = (lipm_height/local_gravity)*dynacore::Matrix::Identity(2, 2); 723 | F_c.block(3, 9, 2, 2) = (lipm_height/local_gravity)*dynacore::Matrix::Identity(2, 2); 724 | }else if(rf_contact){ 725 | F_c.block(3, 0, 3, 3) = (lipm_height/local_gravity)*dynacore::Matrix::Identity(2, 2); 726 | F_c.block(3, 12, 3, 3) = (lipm_height/local_gravity)*dynacore::Matrix::Identity(2, 2); 727 | }else{ 728 | F_c.block(3, 6, 3, 3) = -C_rot.transpose()*getSkewSymmetricMatrix(f_imu_input); 729 | F_c.block(3, 15, 3, 3) = -C_rot.transpose(); 730 | } 731 | 732 | //F_c.block(3, 6, 3, 3) = -C_rot.transpose()*getSkewSymmetricMatrix(f_imu_input); 733 | //F_c.block(3, 15, 3, 3) = -C_rot.transpose(); 734 | F_c.block(6, 6, 3, 3) = -getSkewSymmetricMatrix(omega_imu_input); 735 | F_c.block(6, 18, 3,3) = -dynacore::Matrix::Identity(3,3); 736 | // F_c.block(6, 18, 3,3) = O_q_B.toRotationMatrix(); //-dynacore::Matrix::Identity(3,3); 737 | 738 | // Discretized linear error dynamics 739 | F_k = dynacore::Matrix::Identity(F_c.rows(), F_c.cols()) + F_c*dt; 740 | 741 | // Construct Process noise Jacobian Matrix, L_c 742 | getMatrix_L_c(O_q_B, L_c); 743 | getMatrix_Q(Q_c); 744 | 745 | // Perform Covariance prediction step 746 | P_predicted = F_k*P_prior*F_k.transpose() + F_k*L_c*Q_c*L_c.transpose()*F_k.transpose()*dt; 747 | //P_predicted = L_c*Q_c*L_c.transpose()*dt*dt; 748 | } 749 | 750 | void EKF_LIPRotellaEstimator::predictionStep(){ 751 | statePredictionStep(); 752 | covariancePredictionStep(); 753 | } 754 | 755 | void EKF_LIPRotellaEstimator::getCurrentBodyFrameFootPosition(const int foot_link_id, dynacore::Vector & foot_pos_B){ 756 | // Find Foot body id 757 | unsigned int bodyid; 758 | switch(foot_link_id){ 759 | case mercury_link::leftFoot: 760 | bodyid = robot_model->GetBodyId("lfoot"); 761 | break; 762 | case mercury_link::rightFoot: 763 | bodyid = robot_model->GetBodyId("rfoot"); 764 | break; 765 | default: 766 | printf("[EKF_LIPRotellaEstimator] Not a valid foot link id\n"); 767 | exit(0); 768 | } 769 | 770 | // Set parameters up 771 | dynacore::Vect3 Local_CoM = robot_model->mFixedBodies[ 772 | bodyid - robot_model->fixed_body_discriminator].mCenterOfMass; 773 | 774 | // Align body frame to the fixed frame 775 | dynacore::Vector config_copy = Q_config; 776 | config_copy.head(O_r.size()) = dynacore::Vector::Zero(3); 777 | config_copy[O_r.size()] = 0.0; 778 | config_copy[O_r.size()+1] = 0.0; 779 | config_copy[O_r.size()+2] = 0.0; 780 | config_copy[mercury::num_qdot] = 1.0; 781 | 782 | foot_pos_B = CalcBodyToBaseCoordinates(*robot_model, config_copy, bodyid, Local_CoM, true); 783 | } 784 | 785 | void EKF_LIPRotellaEstimator::getBodyVelFromKinematics(dynacore::Vector & O_body_vel){ 786 | dynacore::Vect3 vel; vel.setZero(); 787 | dynacore::Vect3 zero; zero.setZero(); 788 | O_body_vel = dynacore::Vector::Zero(3); 789 | 790 | unsigned int bodyid; 791 | if (lf_contact && rf_contact){ 792 | bodyid = robot_model->GetBodyId("lfoot"); 793 | }else if (lf_contact){ 794 | bodyid = robot_model->GetBodyId("lfoot"); 795 | }else if (rf_contact){ 796 | bodyid = robot_model->GetBodyId("rfoot"); 797 | }else{ 798 | bodyid = robot_model->GetBodyId("lfoot"); 799 | } 800 | 801 | if(bodyid >=robot_model->fixed_body_discriminator){ 802 | zero = robot_model->mFixedBodies[bodyid - robot_model->fixed_body_discriminator].mCenterOfMass; 803 | } 804 | else{ 805 | zero = robot_model->mBodies[bodyid].mCenterOfMass; 806 | } 807 | 808 | dynacore::Vector config_copy = Q_config; 809 | 810 | config_copy.head(O_r.size()) = O_r; 811 | config_copy[O_r.size()] = O_q_B.x(); 812 | config_copy[O_r.size()+1] = O_q_B.y(); 813 | config_copy[O_r.size()+2] = O_q_B.z(); 814 | config_copy[mercury::num_qdot] = O_q_B.w(); 815 | 816 | dynacore::Vector config_dot_copy = Q_config_dot; 817 | for(int i(0); i<3; ++i){ 818 | config_dot_copy[i] = 0.0; // set 0 velocities for the linear components 819 | config_dot_copy[i+3] = omega_imu_input[i]; 820 | } 821 | 822 | 823 | vel = CalcPointVelocity (*robot_model, config_copy, config_dot_copy, bodyid, zero, true); 824 | 825 | // Set body velocity to negative of foot velocity 826 | for(size_t i = 0; i < 3; i++){ 827 | O_body_vel[i] = -vel[i]; 828 | } 829 | 830 | } 831 | 832 | void EKF_LIPRotellaEstimator::getCoMVelFromKinematics(dynacore::Vector & O_com_vel){ 833 | int start_idx = robot_model->GetBodyId("body"); 834 | double mass = 0.0; 835 | double tot_mass = 0.0; 836 | dynacore::Vect3 link_vel; link_vel.setZero(); 837 | O_com_vel = dynacore::Vector::Zero(3); 838 | 839 | 840 | // Copy configuration and velocity 841 | // Assumes that the following values have been updated 842 | dynacore::Vector config_copy = Q_config; 843 | config_copy.head(O_r.size()) = O_r; 844 | config_copy[O_r.size()] = O_q_B.x(); 845 | config_copy[O_r.size()+1] = O_q_B.y(); 846 | config_copy[O_r.size()+2] = O_q_B.z(); 847 | config_copy[mercury::num_qdot] = O_q_B.w(); 848 | 849 | dynacore::Vector config_dot_copy = Q_config_dot; 850 | config_dot_copy.head(3) = body_vel_kinematics.head(3); 851 | 852 | for(int i(0); i<3; ++i){ 853 | config_dot_copy[i+3] = omega_imu_input[i]; 854 | } 855 | 856 | 857 | 858 | link_vel = CalcPointVelocity ( *robot_model, config_copy, config_dot_copy, start_idx, robot_model->mBodies[start_idx].mCenterOfMass, true); 859 | // Find velocities of all the links and weight it by their mass. 860 | for (int i(start_idx); i< robot_model->mBodies.size() ; ++i){ 861 | mass = robot_model->mBodies[i].mMass; 862 | // CoM velocity Update 863 | link_vel = CalcPointVelocity ( *robot_model, config_copy, config_dot_copy, i, robot_model->mBodies[i].mCenterOfMass, false); 864 | O_com_vel += mass * link_vel; 865 | tot_mass += mass; 866 | } 867 | O_com_vel /= tot_mass; 868 | } 869 | 870 | void EKF_LIPRotellaEstimator::updateStep(){ 871 | // Construct Noise Matrix R 872 | R_c.block(0,0,6,6) = n_p*dynacore::Matrix::Identity(6,6); 873 | 874 | // Get body velocity estimate from kinematics 875 | getBodyVelFromKinematics(body_vel_kinematics); 876 | R_c.block(6,6,3,3) = n_v*dynacore::Matrix::Identity(3,3); 877 | 878 | // Get CoM velocity estimate from kinematics 879 | getCoMVelFromKinematics(body_com_vel_kinematics); 880 | R_c.block(9,9,3,3) = n_com*dynacore::Matrix::Identity(3,3); 881 | 882 | // Get velocity estimate from the IMU 883 | body_imu_vel += (C_rot.transpose()*f_imu + gravity_vec)*dt; 884 | 885 | R_k = R_c/dt; 886 | 887 | // Construct Discretized Observation Matrix 888 | H_k.block(0,0,3,3) = -C_rot; 889 | p_l_B = C_rot*(O_p_l - O_r); // the left foot position in body frame 890 | H_k.block(0,6,3,3) = getSkewSymmetricMatrix(p_l_B); 891 | H_k.block(0,9,3,3) = C_rot; 892 | 893 | H_k.block(3,0,3,3) = -C_rot; 894 | p_r_B = C_rot*(O_p_r - O_r); // the right foot position in body frame 895 | H_k.block(3,6,3,3) = getSkewSymmetricMatrix(p_r_B); 896 | H_k.block(3,12,3,3) = C_rot; 897 | 898 | H_k.block(6, 3, 3, 3) = dynacore::Matrix::Identity(3,3); // Body velocity estimate 899 | H_k.block(9, 3, 3, 3) = dynacore::Matrix::Identity(3,3); // CoM velocity estimate 900 | 901 | //Compute the Kalman Gain 902 | S_k = H_k*P_predicted*H_k.transpose() + R_k; 903 | K_k = P_predicted*H_k.transpose()*(S_k.inverse()); 904 | 905 | // Perform Measurement Using Kinematics 906 | getCurrentBodyFrameFootPosition(mercury_link::leftFoot, z_lfoot_pos_B); 907 | getCurrentBodyFrameFootPosition(mercury_link::rightFoot, z_rfoot_pos_B); 908 | 909 | // Measurement error vector 910 | // y_vec = (z - h()) 911 | y_vec.head(3) = z_lfoot_pos_B - p_l_B; 912 | y_vec.segment(3, 3) = z_rfoot_pos_B - p_r_B; 913 | y_vec.segment(6,3) = body_vel_kinematics - O_v; 914 | y_vec.segment(9,3) = body_com_vel_kinematics - O_v; 915 | 916 | 917 | delta_x_posterior = K_k*y_vec; 918 | //delta_y = H_k*delta_x_posterior; 919 | 920 | // Update State 921 | updateStatePosterior(); 922 | 923 | // Update Covariance Matrix 924 | P_posterior = (dynacore::Matrix::Identity(dim_error_states, dim_error_states) - K_k*H_k)*P_predicted; 925 | P_prior = P_posterior; 926 | 927 | 928 | // Update Prior 929 | //x_prior = x_predicted; 930 | //P_prior = P_predicted; 931 | } 932 | 933 | void EKF_LIPRotellaEstimator::updateStatePosterior(){ 934 | for(size_t i = 0; i < delta_x_posterior.size(); i++){ 935 | if (std::isnan(delta_x_posterior[i])){ 936 | printf("Delta x change Contains isnan value. Will replace this with 0.0 \n"); 937 | delta_x_posterior[i] = 0.0; 938 | printf("Here are the debug statements \n"); 939 | showDebugStatements(); 940 | printf("Quitting program... \n"); 941 | exit(0); 942 | } 943 | } 944 | 945 | x_posterior = x_predicted; 946 | x_posterior.head(O_r.size() + O_v.size()) += delta_x_posterior.head(O_r.size() + O_v.size()); 947 | x_posterior.tail(O_p_l.size() + O_p_r.size() + B_bf.size() + B_bw.size()) += delta_x_posterior.tail(O_p_l.size() + O_p_r.size() + B_bf.size() + B_bw.size()); 948 | 949 | // Prepare quaternion update 950 | dynacore::Quaternion q_prediction, q_change, q_posterior; 951 | q_prediction.x() = x_predicted[O_r.size()+O_v.size()]; 952 | q_prediction.y() = x_predicted[O_r.size()+O_v.size()+1]; 953 | q_prediction.z() = x_predicted[O_r.size()+O_v.size()+2]; 954 | q_prediction.w() = x_predicted[O_r.size()+O_v.size()+3]; 955 | 956 | // Convert delta phi change to quaternion 957 | delta_phi[0] = delta_x_posterior[O_r.size() + O_v.size()]; 958 | delta_phi[1] = delta_x_posterior[O_r.size() + O_v.size() + 1]; 959 | delta_phi[2] = delta_x_posterior[O_r.size() + O_v.size() + 2]; 960 | dynacore::convert(delta_phi, q_change); 961 | 962 | // Perform Quaternion update 963 | q_posterior = dynacore::QuatMultiply(q_change, q_prediction); 964 | q_posterior = q_prediction; 965 | 966 | x_posterior[O_r.size()+O_v.size()] = q_posterior.x(); 967 | x_posterior[O_r.size()+O_v.size()+1] = q_posterior.y(); 968 | x_posterior[O_r.size()+O_v.size()+2] = q_posterior.z(); 969 | x_posterior[O_r.size()+O_v.size()+3] = q_posterior.w(); 970 | 971 | x_prior = x_posterior; 972 | } 973 | 974 | 975 | 976 | void EKF_LIPRotellaEstimator::doFilterCalculations(){ 977 | predictionStep(); 978 | updateStep(); 979 | } 980 | 981 | 982 | 983 | void EKF_LIPRotellaEstimator::showPrintOutStatements(){ 984 | //printf("\n"); 985 | // printf("[EKF Rotella Estimator]\n"); 986 | // dynacore::pretty_print(f_imu, std::cout, "body frame f_imu"); 987 | // dynacore::pretty_print(body_imu_vel, std::cout, "body_imu_vel"); 988 | 989 | // dynacore::pretty_print(omega_imu, std::cout, "body frame omega_imu"); 990 | // dynacore::Vector a_o = (C_rot.transpose()*f_imu_input + gravity_vec); 991 | // dynacore::pretty_print(O_q_B, std::cout, "O_q_B"); 992 | // dynacore::pretty_print(C_rot, std::cout, "C_rot"); 993 | // dynacore::pretty_print(a_o, std::cout, "inertial frame f_imu"); 994 | 995 | // dynacore::pretty_print(omega_imu, std::cout, "body frame angular velocity"); 996 | 997 | // dynacore::pretty_print(f_imu_input, std::cout, "f_imu_input"); 998 | // dynacore::pretty_print(omega_imu_input, std::cout, "omega_imu_input"); 999 | 1000 | // dynacore::pretty_print(x_prior, std::cout, "x_prior"); 1001 | // dynacore::pretty_print(x_predicted, std::cout, "x_predicted"); 1002 | // dynacore::pretty_print(y_vec, std::cout, "y_vec"); 1003 | // dynacore::pretty_print(delta_x_posterior, std::cout, "delta_x_posterior"); 1004 | 1005 | //dynacore::pretty_print(R_k, std::cout, "R_k"); 1006 | // dynacore::pretty_print(delta_phi, std::cout, "delta_phi"); 1007 | // dynacore::pretty_print(x_posterior, std::cout, "x_posterior"); 1008 | 1009 | 1010 | // dynacore::pretty_print(F_c, std::cout, "F_c"); 1011 | //dynacore::pretty_print(F_k, std::cout, "F_k"); 1012 | // dynacore::pretty_print(L_c, std::cout, "L_c"); 1013 | // dynacore::pretty_print(Q_c, std::cout, "Q_c"); 1014 | // dynacore::pretty_print(P_prior, std::cout, "P_prior"); 1015 | // dynacore::pretty_print(H_k, std::cout, "H_k"); 1016 | 1017 | // dynacore::pretty_print(K_k, std::cout, "K_k"); 1018 | 1019 | // dynacore::pretty_print(z_lfoot_pos_B, std::cout, "z_lfoot"); 1020 | // dynacore::pretty_print(z_rfoot_pos_B, std::cout, "z_rfoot"); 1021 | //dynacore::pretty_print(y_vec, std::cout, "y_vec"); 1022 | 1023 | 1024 | // printf("Left Foot contact = %d \n", lf_contact); 1025 | // printf("Right Foot contact = %d \n", rf_contact); 1026 | // dynacore::pretty_print(Q_config, std::cout, "Q_config"); 1027 | } 1028 | 1029 | void EKF_LIPRotellaEstimator::showDebugStatements(){ 1030 | dynacore::pretty_print(f_imu, std::cout, "f_imu"); 1031 | dynacore::pretty_print(omega_imu, std::cout, "omega_imu"); 1032 | 1033 | dynacore::pretty_print(Q_config, std::cout, "Q_config"); 1034 | dynacore::pretty_print(Q_config_dot, std::cout, "Q_config_dot"); 1035 | 1036 | dynacore::pretty_print(x_prior, std::cout, "x_prior"); 1037 | dynacore::pretty_print(x_predicted, std::cout, "x_predicted"); 1038 | dynacore::pretty_print(x_posterior, std::cout, "x_posterior"); 1039 | 1040 | dynacore::pretty_print(S_k, std::cout, "S_k"); 1041 | dynacore::pretty_print(K_k, std::cout, "K_k"); 1042 | } 1043 | -------------------------------------------------------------------------------- /EKF_LIPRotellaEstimator.hpp: -------------------------------------------------------------------------------- 1 | #ifndef EKF_LIP_ROTELLA_ESTIMATOR 2 | #define EKF_LIP_ROTELLA_ESTIMATOR 3 | 4 | #include "EKF_PoseEstimator.hpp" 5 | #include 6 | #include 7 | 8 | 9 | /* This estimator was based on: 10 | 11 | Rotella, Nicholas, et al. 12 | "State estimation for a humanoid robot." 13 | Intelligent Robots and Systems (IROS 2014), 14 | 2014 IEEE/RSJ International Conference on. IEEE, 2014. 15 | 16 | and 17 | 18 | Bloesch, Michael, et al. 19 | "State estimation for legged robots-consistent fusion of leg kinematics and IMU." 20 | Robotics 17 (2013): 17-24. 21 | 22 | But we have since extensively modified it to include body velocity and LIPM dynamics 23 | 24 | */ 25 | 26 | class EKF_LIPRotellaEstimator :public EKF_PoseEstimator{ 27 | public: 28 | EKF_LIPRotellaEstimator(); 29 | ~EKF_LIPRotellaEstimator(); 30 | virtual void setSensorData(const std::vector & acc, 31 | const std::vector & acc_inc, 32 | const std::vector & ang_vel, 33 | const bool & left_foot_contact, 34 | const bool & right_foot_contact, 35 | const dynacore::Vector & joint_values); 36 | 37 | virtual void setSensorData(const std::vector & acc, 38 | const std::vector & acc_inc, 39 | const std::vector & ang_vel, 40 | const bool left_foot_contact, 41 | const bool right_foot_contact, 42 | const dynacore::Vector & joint_values, 43 | const dynacore::Vector & joint_velocity_values); 44 | 45 | virtual void EstimatorInitialization(const dynacore::Quaternion & initial_global_orientation, 46 | const std::vector & initial_imu_acc, 47 | const std::vector & initial_imu_ang_vel); 48 | 49 | 50 | virtual void EstimatorInitializationWithCOMHeight(const dynacore::Quaternion & initial_global_orientation, 51 | const std::vector & initial_imu_acc, 52 | const std::vector & initial_imu_ang_vel, 53 | const double com_height); 54 | 55 | virtual void resetFilter(); 56 | 57 | dynacore::Matrix getSkewSymmetricMatrix(dynacore::Vector & vec_in); 58 | 59 | 60 | void handleFootContacts(); 61 | void computeNewFootLocations(const int foot_link_id); 62 | 63 | void setStateVariablesToPrior(); 64 | void setStateVariablesToPredicted(); 65 | 66 | void showPrintOutStatements(); 67 | 68 | void showDebugStatements(); 69 | 70 | void getMatrix_L_c(const dynacore::Quaternion & q_in, dynacore::Matrix & L_c_mat); 71 | void getMatrix_Q(dynacore::Matrix & Q_mat); 72 | void statePredictionStep(); 73 | void covariancePredictionStep(); 74 | 75 | void getCurrentBodyFrameFootPosition(const int foot_link_id, dynacore::Vector & foot_pos_B); 76 | 77 | void getBodyVelFromKinematics(dynacore::Vector & O_body_vel); 78 | 79 | void getCoMVelFromKinematics(dynacore::Vector & O_com_vel); 80 | 81 | 82 | void updateStatePosterior(); 83 | 84 | void predictionStep(); 85 | void updateStep(); 86 | void doFilterCalculations(); 87 | 88 | 89 | 90 | void calibrateOrientationFromGravity(); 91 | 92 | protected: 93 | dynacore::Vector O_p_l; // global left foot position 94 | dynacore::Vector O_p_r; // global right foot position 95 | 96 | dynacore::Vector B_bf; // imu frame acceleration bias 97 | dynacore::Vector B_bw; // imu frame angular velocity bias 98 | 99 | dynacore::Vector f_imu; // imu frame acceleration 100 | dynacore::Vector omega_imu; // imu frame angular velocity 101 | 102 | dynacore::Vector f_imu_input; // imu frame acceleration 103 | dynacore::Vector omega_imu_input; // imu frame angular velocity 104 | 105 | bool initial_foot_contact; // checks whether an initial foot contact at startup has occured for the first time. 106 | 107 | bool lf_contact; // value of left foot contact 108 | bool rf_contact; // value of right foot contact 109 | 110 | bool prev_lf_contact; // previous value of the left foot contact 111 | bool prev_rf_contact; // previous value of the right foot contact 112 | 113 | RigidBodyDynamics::Model* robot_model; 114 | 115 | dynacore::Vector Q_config; // configuration of the robot_model 116 | dynacore::Vector Q_config_dot; // configuration velocity of the robot model 117 | 118 | 119 | void _SetupParameters(); 120 | int count; 121 | 122 | double lipm_height; 123 | 124 | // EKF Variables----------------------------------- 125 | double local_gravity; 126 | dynacore::Vector gravity_vec; 127 | double dt; 128 | 129 | int dim_states; 130 | int dim_rvq_states; 131 | int dim_process_errors; 132 | int dim_error_states; 133 | int dim_obs; 134 | int dim_inputs; 135 | 136 | dynacore::Matrix C_rot; // rotation matrix from inerital frame to body frame. 137 | 138 | dynacore::Matrix F_c; // contiouous prediction Jacobian. aka: f_x (linearized error state dynamics) 139 | dynacore::Matrix L_c; // continuous process error Jacobian 140 | dynacore::Matrix Q_c; // continuous process noise 141 | double wf_intensity; // imu process noise intensity 142 | double ww_intensity; // angular velocity noise intensity 143 | double wp_l_intensity; // left foot location noise intensity 144 | double wp_r_intensity; // right foot location noise intensity 145 | 146 | double wp_intensity_default; // default foot location noise intensity 147 | double wp_intensity_unknown; // noise intensity when there is no foot contact 148 | 149 | double wbf_intensity; // imu bias intensity 150 | double wbw_intensity; // angular velocity bias intensity 151 | 152 | dynacore::Matrix F_k; // discretized error state prediction matrix 153 | dynacore::Matrix H_k; // discretized error state observation matrix 154 | 155 | dynacore::Matrix R_c; // Measurement noise covariance matrix 156 | dynacore::Matrix R_k; // Discretized measurement noise covariance matrix 157 | double n_p; // Measurement noise intensity 158 | 159 | 160 | double n_v_default; // default noise intensity of body velocity measurement 161 | double n_v_unknown; // unknown noise intensity from body velocity measurement 162 | double n_v; // Body velocity from kinematics measurement noise intensity 163 | 164 | double n_com_default; // default noise intensity of com velocity measurement 165 | double n_com_unknown; // unknown noise intensity from com velocity measurement 166 | double n_com; // CoM velocity from kinematics measurement noise intensity 167 | 168 | dynacore::Vector body_vel_kinematics; // kinematics and angular velocity based body velocity measurement 169 | dynacore::Vector body_com_vel_kinematics; // kinematics and angular velocity based com velocity measurement 170 | dynacore::Vector body_imu_vel; // kinematics and angular velocity based com velocity measurement 171 | 172 | dynacore::Matrix S_k; // Innovation (residual) covariance 173 | dynacore::Matrix K_k; // Kalman gain 174 | 175 | dynacore::Matrix P_prior; // Prior covariance matrix 176 | dynacore::Matrix P_predicted; // Predicted covariance matrix 177 | dynacore::Matrix P_posterior; // Posterior covariance matrix 178 | 179 | dynacore::Vector x_prior; // Prior EKF States 180 | dynacore::Vector x_predicted; // Predicted EKF States 181 | dynacore::Vector x_posterior; // Posterior EKF States 182 | 183 | dynacore::Vector delta_x_prior; // Prior error states 184 | dynacore::Vector delta_x_posterior; // Posterier error states 185 | dynacore::Vector delta_y; // prediction and measurement differences 186 | 187 | dynacore::Vector z_lfoot_pos_B; // Measured body frame left foot position 188 | dynacore::Vector z_rfoot_pos_B; // Measured body frame right foot position 189 | 190 | dynacore::Vector p_l_B; // Body frame left foot position 191 | dynacore::Vector p_r_B; // Body frame right foot position 192 | 193 | dynacore::Vector y_vec; // prediction and measurement differences 194 | double weight_body_vel_obs; 195 | double weight_com_vel_obs; 196 | 197 | dynacore::Vect3 delta_phi; 198 | 199 | // Initialize Orientation Calibration Variables 200 | double theta_x; 201 | double theta_y; 202 | double gravity_mag; 203 | double roll_value_comp; 204 | double pitch_value_comp; 205 | 206 | dynacore::Vect3 g_B; 207 | dynacore::Vect3 g_B_local_vec; // rotated gravity direction 208 | dynacore::Quaternion Oq_B_init; // initial quaternion of the body frame w.r.t fixed frame 209 | dynacore::Matrix OR_B_init; // initial Rot matrix of body w.r.t fixed frame 210 | dynacore::Quaternion q_world_Rx; 211 | dynacore::Quaternion q_world_Ry; 212 | 213 | double bias_lp_frequency_cutoff; 214 | digital_lp_filter* x_bias_low_pass_filter; 215 | digital_lp_filter* y_bias_low_pass_filter; 216 | digital_lp_filter* z_bias_low_pass_filter; 217 | double x_acc_bias; 218 | double y_acc_bias; 219 | double z_acc_bias; 220 | 221 | 222 | }; 223 | 224 | 225 | #endif 226 | -------------------------------------------------------------------------------- /EKF_RotellaEstimator.cpp: -------------------------------------------------------------------------------- 1 | #include "EKF_RotellaEstimator.hpp" 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | using namespace RigidBodyDynamics; 10 | 11 | EKF_RotellaEstimator::EKF_RotellaEstimator():Q_config(mercury::num_q), 12 | Q_config_dot(mercury::num_qdot), 13 | O_p_l(3), 14 | O_p_r(3), 15 | B_bf(3), 16 | B_bw(3), 17 | initial_foot_contact(false), 18 | lf_contact(false), 19 | rf_contact(false), 20 | prev_lf_contact(false), 21 | prev_rf_contact(false), 22 | f_imu(3), 23 | omega_imu(3) 24 | { 25 | // Load Robot Model 26 | robot_model = new Model(); 27 | rbdl_check_api_version (RBDL_API_VERSION); 28 | if (!Addons::URDFReadFromFile (THIS_COM"/RobotSystems/Mercury/mercury.urdf", robot_model, false)) { 29 | std::cerr << "Error loading model ./mercury.urdf" << std::endl; 30 | abort(); 31 | } 32 | // -------------------- 33 | printf("[EKF Rotella Estimator] Successfully loaded URDF robot model."); 34 | 35 | // Initialize Global Positions 36 | O_r = dynacore::Vector::Zero(3); 37 | O_v = dynacore::Vector::Zero(3); 38 | 39 | Q_config.setZero(); 40 | Q_config_dot.setZero(); 41 | 42 | // Initialize Orientations 43 | O_q_B.w() = 1; 44 | O_q_B.x() = 0.0; O_q_B.y() = 0.0; O_q_B.z() = 0.0; 45 | Q_config[mercury::num_qdot] = 1.0; // Set orientation to identity. 46 | 47 | 48 | // Initialize remaining state vectors 49 | O_p_l.setZero(); 50 | O_p_r.setZero(); 51 | B_bf.setZero(); 52 | B_bw.setZero(); 53 | 54 | // Initialize inputs 55 | f_imu.setZero(); 56 | omega_imu.setZero(); 57 | 58 | count = 0; 59 | // Initialize EKF Variables 60 | local_gravity = 9.81; 61 | gravity_vec = dynacore::Vector::Zero(3); 62 | gravity_vec[2] = local_gravity; 63 | 64 | dt = mercury::servo_rate; 65 | dim_states = O_r.size() + O_v.size() + 4 + O_p_l.size() + O_p_r.size() + B_bf.size() + B_bw.size(); 66 | dim_rvq_states = O_r.size() + O_v.size() + 4; 67 | 68 | 69 | dim_error_states = O_r.size() + O_v.size() + 3 + O_p_l.size() + O_p_r.size() + B_bf.size() + B_bw.size(); 70 | dim_process_errors = f_imu.size() + omega_imu.size() + O_p_l.size() + O_p_r.size() + B_bf.size() + B_bw.size(); 71 | dim_obs = O_p_l.size() + O_p_r.size() + O_v.size(); 72 | dim_inputs = f_imu.size() + omega_imu.size(); 73 | 74 | 75 | 76 | C_rot = dynacore::Matrix::Identity(3,3); 77 | F_c = dynacore::Matrix::Zero(dim_error_states, dim_error_states); 78 | L_c = dynacore::Matrix::Zero(dim_error_states, dim_process_errors); 79 | Q_c = dynacore::Matrix::Zero(dim_process_errors,dim_process_errors); 80 | 81 | F_k = dynacore::Matrix::Zero(dim_error_states, dim_error_states); 82 | H_k = dynacore::Matrix::Zero(dim_obs, dim_error_states); 83 | 84 | R_c = dynacore::Matrix::Zero(dim_obs, dim_obs); 85 | R_k = dynacore::Matrix::Zero(dim_obs, dim_obs); 86 | 87 | S_k = dynacore::Matrix::Zero(dim_obs, dim_obs); 88 | K_k = dynacore::Matrix::Zero(dim_error_states, dim_obs); 89 | 90 | P_prior = dynacore::Matrix::Zero(dim_error_states, dim_error_states); 91 | P_predicted = dynacore::Matrix::Zero(dim_error_states, dim_error_states); 92 | P_posterior = dynacore::Matrix::Zero(dim_error_states, dim_error_states); 93 | 94 | x_prior = dynacore::Vector::Zero(dim_states); 95 | x_predicted = dynacore::Vector::Zero(dim_states); 96 | x_posterior = dynacore::Vector::Zero(dim_states); 97 | x_prior[9] = 1.0; // Set Quaternion to identity. 98 | x_posterior[9] = 1.0; // Set Quaternion to identity. 99 | 100 | 101 | delta_x_prior = dynacore::Vector::Zero(dim_error_states); 102 | delta_x_posterior = dynacore::Vector::Zero(dim_error_states); 103 | 104 | delta_phi.setZero(); 105 | 106 | delta_y = dynacore::Vector::Zero(dim_obs); 107 | 108 | z_lfoot_pos_B = dynacore::Vector::Zero(O_p_l.size()); 109 | z_rfoot_pos_B = dynacore::Vector::Zero(O_p_r.size()); 110 | 111 | p_l_B = dynacore::Vector::Zero(O_p_l.size()); 112 | p_r_B = dynacore::Vector::Zero(O_p_r.size()); 113 | 114 | body_vel_kinematics = dynacore::Vector::Zero(O_v.size()); 115 | 116 | y_vec = dynacore::Vector::Zero(dim_obs); 117 | 118 | // Initialize Covariance parameters 119 | // Values are from reference paper. Need to be changed to known IMU parameters 120 | ww_intensity = 0.000523; // rad/s/sqrt(Hz) // angular velocity process noise intensity 121 | 122 | wp_intensity_default = 0.001;//0.001; // m/sqrt(Hz) // default foot location noise intensity 123 | wp_intensity_unknown = 1000.0; // m/sqrt(Hz) // noise intensity when there is no foot contact 124 | wp_l_intensity = wp_intensity_unknown; // m/sqrt(Hz) // left foot location noise intensity 125 | wp_r_intensity = wp_intensity_unknown; // m/sqrt(Hz) // right foot location noise intensity 126 | 127 | // Simulation params 128 | wf_intensity = 0.001;//0.00078; // m/(s^2)/sqrt(Hz) // imu process noise intensity 129 | wbf_intensity = 10.0; // m/(s^3)/sqrt(Hz) // imu bias intensity 130 | 131 | // Real sensor params 132 | // wf_intensity = 0.01;//0.00078; // m/(s^2)/sqrt(Hz) // imu process noise intensity 133 | // wbf_intensity = 10.0; // m/(s^3)/sqrt(Hz) // imu bias intensity 134 | wbw_intensity = 0.000618; // rad/(s^2)/sqrt(Hz) // ang vel bias intensity 135 | 136 | n_p = 0.01; // foot measurement noise intensity. 137 | 138 | n_v_default = 0.01; // default noise intensity of body velocity measurement 139 | n_v_unknown = 1000; // unknown noise intensity from body velocity measurement 140 | 141 | n_v = n_v_default; // Body velocity from kinematics measurement noise intensity 142 | 143 | // Initialize Orientation Calibration Filters 144 | // Bias Filter 145 | bias_lp_frequency_cutoff = 2.0*3.1415*1.0; // 1Hz // (2*pi*frequency) rads/s 146 | x_bias_low_pass_filter = new digital_lp_filter(bias_lp_frequency_cutoff, mercury::servo_rate); 147 | y_bias_low_pass_filter = new digital_lp_filter(bias_lp_frequency_cutoff, mercury::servo_rate); 148 | z_bias_low_pass_filter = new digital_lp_filter(bias_lp_frequency_cutoff, mercury::servo_rate); 149 | 150 | // Initialize body orientation to identity. 151 | dynacore::Vect3 rpy_init; rpy_init.setZero(); 152 | dynacore::convert(rpy_init, Oq_B_init); 153 | OR_B_init = Oq_B_init.toRotationMatrix(); 154 | 155 | // Initialize other calibration parameters 156 | gravity_mag = 9.81; // m/s^2; 157 | theta_x = 0.0; 158 | theta_y = 0.0; 159 | 160 | x_acc_bias = 0.0; 161 | y_acc_bias = 0.0; 162 | z_acc_bias = 0.0; 163 | 164 | g_B.setZero(); 165 | g_B_local_vec.setZero(); 166 | 167 | // Register data for logging 168 | 169 | DataManager::GetDataManager()->RegisterData(&O_r, DYN_VEC, "ekf_o_r", 3); 170 | DataManager::GetDataManager()->RegisterData(&O_v, DYN_VEC, "ekf_o_v", 3); 171 | DataManager::GetDataManager()->RegisterData(&O_q_B, QUATERNION, "ekf_o_q_b", 4); 172 | DataManager::GetDataManager()->RegisterData(&f_imu, DYN_VEC, "ekf_f_imu", 3); 173 | DataManager::GetDataManager()->RegisterData(&omega_imu, DYN_VEC, "ekf_omega_imu", 3); 174 | DataManager::GetDataManager()->RegisterData(&B_bf, DYN_VEC, "ekf_B_bf", 3); 175 | DataManager::GetDataManager()->RegisterData(&B_bw, DYN_VEC, "ekf_B_bw", 3); 176 | DataManager::GetDataManager()->RegisterData(&y_vec, DYN_VEC, "ekf_measured_diff", 6); 177 | 178 | DataManager::GetDataManager()->RegisterData(&z_lfoot_pos_B, DYN_VEC, "ekf_z_l_foot_B", 3); 179 | DataManager::GetDataManager()->RegisterData(&z_rfoot_pos_B, DYN_VEC, "ekf_z_r_foot_B", 3); 180 | DataManager::GetDataManager()->RegisterData(&p_l_B, DYN_VEC, "ekf_p_left_B", 3); 181 | DataManager::GetDataManager()->RegisterData(&p_r_B, DYN_VEC, "ekf_p_right_B", 3); 182 | 183 | DataManager::GetDataManager()->RegisterData(&O_p_l, DYN_VEC, "ekf_p_left_O", 3); 184 | DataManager::GetDataManager()->RegisterData(&O_p_r, DYN_VEC, "ekf_p_right_O", 3); 185 | 186 | 187 | DataManager::GetDataManager()->RegisterData(&body_vel_kinematics, DYN_VEC, "ekf_body_vel_kin", 3); 188 | 189 | 190 | } 191 | 192 | 193 | EKF_RotellaEstimator::~EKF_RotellaEstimator(){ 194 | delete robot_model; 195 | } 196 | 197 | void EKF_RotellaEstimator::EstimatorInitialization(const dynacore::Quaternion & initial_global_orientation, 198 | const std::vector & initial_imu_acc, 199 | const std::vector & initial_imu_ang_vel){ 200 | 201 | 202 | // Initialize IMU values 203 | for(size_t i = 0; i < 3; i++){ 204 | f_imu[i] = -initial_imu_acc[i]; 205 | omega_imu[i] = initial_imu_ang_vel[i]; 206 | } 207 | 208 | // Initialize Covariance matrix prior 209 | getMatrix_L_c(O_q_B, L_c); 210 | getMatrix_Q(Q_c); 211 | P_prior = L_c*Q_c*L_c.transpose();// L_c*Q_c; 212 | 213 | // Initialize Global Orientation 214 | //O_q_B = initial_global_orientation; 215 | 216 | // // Initialize global orientation 217 | calibrateOrientationFromGravity(); 218 | 219 | x_prior[O_r.size()+O_v.size()] = O_q_B.x(); 220 | x_prior[O_r.size()+O_v.size()+1] = O_q_B.y(); 221 | x_prior[O_r.size()+O_v.size()+2] = O_q_B.z(); 222 | x_prior[O_r.size()+O_v.size()+3] = O_q_B.w(); 223 | 224 | } 225 | 226 | void EKF_RotellaEstimator::calibrateOrientationFromGravity(){ 227 | //printf("[EKF Rotella Estimator] Initializing Orientation from IMU acceleration data. The robot should not be moving \n"); 228 | 229 | // Update bias estimate 230 | x_bias_low_pass_filter->input(f_imu[0]); 231 | y_bias_low_pass_filter->input(f_imu[1]); 232 | z_bias_low_pass_filter->input(f_imu[2]); 233 | 234 | x_acc_bias = x_bias_low_pass_filter->output(); 235 | y_acc_bias = y_bias_low_pass_filter->output(); 236 | z_acc_bias = z_bias_low_pass_filter->output(); 237 | 238 | // Finds The orientation of the body with respect to the fixed frame O ((^OR_B ). 239 | // This assumes that the IMU can only sense gravity as the acceleration. 240 | // 241 | // The algorithm attempts to solve ^OR_B * f_b = f_o, 242 | // where f_b is the acceleration of gravity in the body frame. 243 | // f_o is the acceleration of gravity in the fixed frame 244 | // In addition to ^OR_B acting as a change of frame formula, 245 | // note that ^OR_B will also equal to the orientation of the body w.r.t to the fixed frame. 246 | 247 | // We will rotate f_b using an extrinsic rotation with global R_x (roll) and R_y (pitch) rotations 248 | // Thus, we will perform ^OR_y ^OR_x f_b = f_o. 249 | 250 | // Finally, note that ^OR_b = ^OR_y ^OR_x. 251 | // The resulting orientation will have the xhat component of ^OR_b (ie: ^OR_b.col(0)) to be always planar 252 | // with the inertial x-z plane. 253 | // 254 | // It is best to visualize the extrinsic rotation on paper for any given extrinsic roll then pitch operations 255 | 256 | g_B.setZero(); 257 | g_B[0] = x_acc_bias; 258 | g_B[1] = y_acc_bias; 259 | g_B[2] = z_acc_bias; // We expect a negative number if gravity is pointing opposite of the IMU zhat direction 260 | 261 | gravity_mag = g_B.norm(); 262 | g_B /= gravity_mag; 263 | 264 | // Prepare to rotate gravity vector 265 | g_B_local_vec[0] = g_B[0]; g_B_local_vec[1] = g_B[1]; g_B_local_vec[2] = g_B[2]; 266 | 267 | //--------------------------------------------------------------------------- 268 | // Use Rx to rotate the roll and align gravity vector - 269 | // Compute Roll to rotate 270 | // theta_x = atan(g_B[1]/g_B[2]); 271 | // double roll_val = theta_x; 272 | 273 | // The following method can handle any initial vector due to gravity 274 | theta_x = atan2(g_B_local_vec[2], g_B_local_vec[1]); // Returns angle \in [-pi, pi] between z and y projected vectors. 275 | double roll_val = (-M_PI/2.0 - theta_x); // (-pi/2 - theta_x) 276 | roll_value_comp = roll_val; 277 | 278 | //dynacore::convert(0.0, 0.0, roll_val, q_world_Rx); 279 | // Create Roll Quaternion 280 | q_world_Rx.w() = cos(roll_val/2.);; 281 | q_world_Rx.x() = sin(roll_val/2.); 282 | q_world_Rx.y() = 0; 283 | q_world_Rx.z() = 0; 284 | 285 | //Rotate gravity vector to align the yhat directions 286 | dynacore::Matrix Rx = q_world_Rx.normalized().toRotationMatrix(); 287 | g_B_local_vec = Rx*g_B_local_vec; 288 | 289 | // Use Ry to rotate pitch and align gravity vector --------------------------- 290 | // Compute Pitch to rotate 291 | // theta_y = atan(g_B_local.x()/g_B_local.z()); 292 | // double pitch_val = -theta_y; 293 | 294 | // The following method can handle any initial vector due to gravity 295 | theta_y = atan2(g_B_local_vec[2], g_B_local_vec[0]); // Returns angle \in [-pi, pi] between z and x projected vectors. 296 | double pitch_val = -((-M_PI/2.0) - theta_y); // This is actually -(-pi/2 - theta_y) 297 | pitch_value_comp = pitch_val; 298 | 299 | //dynacore::convert(0.0, pitch_val, 0.0, q_world_Ry); 300 | // Create Pitch Quaternion 301 | q_world_Ry.w() = cos(pitch_val/2.); 302 | q_world_Ry.x() = 0; 303 | q_world_Ry.y() = sin(pitch_val/2.); 304 | q_world_Ry.z() = 0; 305 | 306 | // Rotate gravity vector to align the xhat directions 307 | dynacore::Matrix Ry = q_world_Ry.normalized().toRotationMatrix(); 308 | g_B_local_vec = Ry*g_B_local_vec; 309 | 310 | // Obtain initial body orientation w.r.t fixed frame. 311 | //Oq_B = q_y * q_x * q_b 312 | Oq_B_init = dynacore::QuatMultiply(q_world_Ry, q_world_Rx); 313 | // Set rotation matrix 314 | OR_B_init = Oq_B_init.normalized().toRotationMatrix(); 315 | 316 | // Initialize orientation 317 | O_q_B = Oq_B_init; 318 | 319 | // dynacore::pretty_print(g_B, std::cout, "gravity_dir"); 320 | // printf(" gravity_mag = %0.4f \n", gravity_mag); 321 | // printf(" theta_x = %0.4f \n", theta_x); 322 | // printf(" theta_y = %0.4f \n", theta_y); 323 | // printf(" roll_value_comp = %0.4f \n", roll_value_comp); 324 | // printf(" pitch_value_comp = %0.4f \n", pitch_value_comp); 325 | // dynacore::pretty_print(g_B_local_vec, std::cout, "rotated gravity vec"); 326 | 327 | // dynacore::pretty_print(Oq_B_init, std::cout, "Initial body orientation w.r.t fixed frame: "); 328 | // dynacore::pretty_print(OR_B_init, std::cout, "OR_B_init: "); 329 | // dynacore::pretty_print(O_q_B, std::cout, "Body orientation w.r.t fixed frame: "); 330 | // printf("\n"); 331 | 332 | } 333 | 334 | 335 | void EKF_RotellaEstimator::computeNewFootLocations(const int foot_link_id){ 336 | // Find Foot body id 337 | unsigned int bodyid; 338 | switch(foot_link_id){ 339 | case mercury_link::leftFoot: 340 | bodyid = robot_model->GetBodyId("lfoot"); 341 | break; 342 | case mercury_link::rightFoot: 343 | bodyid = robot_model->GetBodyId("rfoot"); 344 | break; 345 | default: 346 | printf("[EKF_RotellaEstimator] Not a valid foot link id\n"); 347 | exit(0); 348 | } 349 | 350 | // Set parameters up 351 | dynacore::Vect3 Local_CoM = robot_model->mFixedBodies[ 352 | bodyid - robot_model->fixed_body_discriminator].mCenterOfMass; 353 | 354 | Q_config.head(O_r.size()) = O_r; 355 | Q_config[O_r.size()] = O_q_B.x(); 356 | Q_config[O_r.size()+1] = O_q_B.y(); 357 | Q_config[O_r.size()+2] = O_q_B.z(); 358 | Q_config[mercury::num_qdot] = O_q_B.w(); 359 | 360 | // Update new foot locations 361 | if (foot_link_id == mercury_link::leftFoot){ 362 | O_p_l = CalcBodyToBaseCoordinates(*robot_model, Q_config, bodyid, Local_CoM, true); 363 | x_prior.segment(dim_rvq_states, O_p_l.size()) = O_p_l; 364 | } 365 | if (foot_link_id == mercury_link::rightFoot){ 366 | O_p_r = CalcBodyToBaseCoordinates(*robot_model, Q_config, bodyid, Local_CoM, true); 367 | x_prior.segment(dim_rvq_states + O_p_l.size(), O_p_r.size()) = O_p_r; 368 | } 369 | 370 | 371 | } 372 | 373 | void EKF_RotellaEstimator::resetFilter(){ 374 | // Initialize Covariance matrix prior 375 | getMatrix_L_c(O_q_B, L_c); 376 | getMatrix_Q(Q_c); 377 | P_prior = L_c*Q_c*L_c.transpose(); 378 | 379 | // // Initialize state vector except for the orientation 380 | x_prior.head(O_r.size() + O_v.size()) = dynacore::Vector::Zero(O_r.size() + O_v.size()); 381 | x_prior.tail(O_p_l.size() + O_p_r.size() + B_bf.size() + B_bw.size()) = dynacore::Vector::Zero(O_p_l.size() + O_p_r.size() + B_bf.size() + B_bw.size()); 382 | 383 | setStateVariablesToPrior(); 384 | // x_prior = dynacore::Vector::Zero(dim_states); 385 | // x_prior[9] = 1.0; 386 | 387 | computeNewFootLocations(mercury_link::leftFoot); // Update Left foot location 388 | computeNewFootLocations(mercury_link::rightFoot); // Update Right foot location 389 | 390 | 391 | } 392 | 393 | void EKF_RotellaEstimator::setSensorData(const std::vector & acc, 394 | const std::vector & acc_inc, 395 | const std::vector & ang_vel, 396 | const bool left_foot_contact, 397 | const bool right_foot_contact, 398 | const dynacore::Vector & joint_values, 399 | const dynacore::Vector & joint_velocity_values){ 400 | 401 | Q_config_dot.segment(mercury::num_virtual, mercury::num_act_joint) = joint_velocity_values; 402 | setSensorData(acc, acc_inc, ang_vel, left_foot_contact, right_foot_contact, joint_values); 403 | } 404 | 405 | void EKF_RotellaEstimator::setSensorData(const std::vector & acc, 406 | const std::vector & acc_inc, 407 | const std::vector & ang_vel, 408 | const bool & left_foot_contact, 409 | const bool & right_foot_contact, 410 | const dynacore::Vector & joint_values){ 411 | 412 | for(size_t i = 0; i < 3; i++){ 413 | f_imu[i] = -acc[i]; 414 | omega_imu[i] = ang_vel[i]; 415 | } 416 | //f_imu[i] = acc[i]; 417 | //f_imu[2] = -f_imu[2]; 418 | 419 | lf_contact = left_foot_contact; 420 | rf_contact = right_foot_contact; 421 | Q_config.segment(mercury::num_virtual, mercury::num_act_joint) = joint_values; 422 | 423 | // Note: the first time contact is activated, the estimator is reset. 424 | if ( (!initial_foot_contact) && ((lf_contact) || (rf_contact)) ) { 425 | printf("\n"); 426 | printf("[EKF_RotellaEstimator] initial contact triggered. EKF filter will be reset. \n"); 427 | printf("\n"); 428 | initial_foot_contact = true; 429 | resetFilter(); 430 | } 431 | 432 | 433 | // Handle changes in foot contact 434 | handleFootContacts(); 435 | 436 | // Perform filter calculations given sensor data 437 | doFilterCalculations(); 438 | 439 | //Print Statements 440 | if (count % 100 == 0){ 441 | showPrintOutStatements(); 442 | count = 0; 443 | } 444 | count++; 445 | 446 | // Update foot contact booleans 447 | prev_lf_contact = lf_contact; 448 | prev_rf_contact = rf_contact; 449 | 450 | } 451 | 452 | void EKF_RotellaEstimator::handleFootContacts(){ 453 | // if the foot location is lost, set wp to a high number 454 | if (lf_contact){ 455 | wp_l_intensity = wp_intensity_default; 456 | }else{ 457 | wp_l_intensity = wp_intensity_unknown; 458 | } 459 | // if the foot location is lost, set wp to a high number 460 | if (rf_contact){ 461 | wp_r_intensity = wp_intensity_default; 462 | }else{ 463 | wp_r_intensity = wp_intensity_unknown; 464 | } 465 | 466 | // Set n_v to a high number if neither foot contact is available 467 | if (lf_contact || rf_contact){ 468 | n_v = n_v_default; 469 | }else{ 470 | n_v = n_v_unknown; 471 | } 472 | 473 | // Handle new contact detections 474 | // Check if a new foot location will be used for estimation 475 | if ((prev_lf_contact == false) && (lf_contact == true)){ 476 | // printf("\n New Left foot contact\n"); 477 | computeNewFootLocations(mercury_link::leftFoot); // Update Left foot location 478 | // Update Prior? 479 | //P_prior.block(9,9,3,3) = wp_intensity_default*dynacore::Matrix::Identity(3,3); 480 | 481 | } 482 | if ((prev_rf_contact == false) && (rf_contact == true)){ 483 | // printf("\n New Right foot contact\n"); 484 | computeNewFootLocations(mercury_link::rightFoot); // Update right foot location 485 | // Update Prior? 486 | //P_prior.block(12,12,3,3) = wp_intensity_default*dynacore::Matrix::Identity(3,3); 487 | } 488 | 489 | // Handle loss of contact 490 | // Update Covariance when a loss in contact is detected 491 | if ((prev_lf_contact == true) && (lf_contact == false)){ 492 | //P_prior.block(9,9,3,3) = wp_intensity_unknown*dynacore::Matrix::Identity(3,3); 493 | } 494 | if ((prev_rf_contact == true) && (rf_contact == false)){ 495 | //P_prior.block(12,12,3,3) = wp_intensity_unknown*dynacore::Matrix::Identity(3,3); 496 | } 497 | } 498 | 499 | dynacore::Matrix EKF_RotellaEstimator::getSkewSymmetricMatrix(dynacore::Vector & vec_in){ 500 | dynacore::Matrix ssm = dynacore::Matrix::Zero(3,3); 501 | ssm(0,1) = -vec_in[2]; ssm(0,2) = vec_in[1]; 502 | ssm(1,0) = vec_in[2]; ssm(1,2) = -vec_in[0]; 503 | ssm(2,0) = -vec_in[1]; ssm(2,1) = vec_in[0]; 504 | return ssm; 505 | } 506 | 507 | void EKF_RotellaEstimator::setStateVariablesToPrior(){ 508 | // Get prior values: 509 | O_r = x_prior.head(O_r.size()); 510 | O_v = x_prior.segment(O_r.size(), O_v.size()); 511 | O_q_B.x() = x_prior[O_r.size() + O_v.size()]; 512 | O_q_B.y() = x_prior[O_r.size() + O_v.size() + 1]; 513 | O_q_B.z() = x_prior[O_r.size() + O_v.size() + 2]; 514 | O_q_B.w() = x_prior[O_r.size() + O_v.size() + 3]; 515 | 516 | C_rot = O_q_B.toRotationMatrix(); 517 | 518 | O_p_l = x_prior.segment(dim_rvq_states, O_p_l.size()); 519 | O_p_r = x_prior.segment(dim_rvq_states + O_p_l.size(), O_p_r.size()); 520 | B_bf = x_prior.segment(dim_rvq_states + O_p_l.size() + O_p_r.size(), B_bf.size()); 521 | B_bw = x_prior.segment(dim_rvq_states + O_p_l.size() + O_p_r.size() + B_bw.size(), B_bw.size()); 522 | 523 | B_bf = x_prior.segment(dim_rvq_states + O_p_l.size() + O_p_r.size(), B_bf.size()); 524 | B_bw = x_prior.segment(dim_rvq_states + O_p_l.size() + O_p_r.size() + B_bw.size(), B_bw.size()); 525 | 526 | } 527 | 528 | void EKF_RotellaEstimator::setStateVariablesToPredicted(){ 529 | O_r = x_predicted.head(O_r.size()); 530 | O_v = x_predicted.segment(O_r.size(), O_v.size()); 531 | O_q_B.x() = x_predicted[O_r.size() + O_v.size()]; 532 | O_q_B.y() = x_predicted[O_r.size() + O_v.size() + 1]; 533 | O_q_B.z() = x_predicted[O_r.size() + O_v.size() + 2]; 534 | O_q_B.w() = x_predicted[O_r.size() + O_v.size() + 3]; 535 | 536 | C_rot = O_q_B.toRotationMatrix(); 537 | 538 | O_p_l = x_predicted.segment(dim_rvq_states, O_p_l.size()); 539 | O_p_r = x_predicted.segment(dim_rvq_states + O_p_l.size(), O_p_r.size()); 540 | B_bf = x_predicted.segment(dim_rvq_states + O_p_l.size() + O_p_r.size(), B_bf.size()); 541 | B_bw = x_predicted.segment(dim_rvq_states + O_p_l.size() + O_p_r.size() + B_bf.size(), B_bw.size()); 542 | } 543 | 544 | void EKF_RotellaEstimator::statePredictionStep(){ 545 | // Prepare state variables 546 | setStateVariablesToPrior(); 547 | 548 | // Prepare filter input: 549 | //f_imu_input = C_rot * f_imu; 550 | f_imu_input = f_imu - B_bf; 551 | // omega_imu_input = omega_imu - B_bw; 552 | omega_imu_input = omega_imu; 553 | 554 | // Prepare rotation values 555 | dynacore::Quaternion B_q_omega; 556 | dynacore::Vect3 vec3_omega_input; vec3_omega_input.setZero(); 557 | for(size_t i = 0; i < 3; i++){ 558 | vec3_omega_input[i] = omega_imu_input[i]; 559 | } 560 | dynacore::convert(vec3_omega_input*dt, B_q_omega); 561 | 562 | // Perform Discrete State prediction step; 563 | // predict position and velocity 564 | x_predicted.segment(0, O_r.size()) = O_r + O_v*dt + 0.5*dt*dt*(C_rot.transpose()*f_imu_input + gravity_vec); 565 | x_predicted.segment(O_r.size(), O_v.size()) = O_v + dt*(C_rot.transpose()*f_imu_input + gravity_vec); 566 | 567 | // predict orientation 568 | dynacore::Quaternion q_predicted = dynacore::QuatMultiply(O_q_B, B_q_omega); 569 | // dynacore::Quaternion q_predicted = dynacore::QuatMultiply(B_q_omega, O_q_B); 570 | 571 | x_predicted[O_r.size()+O_v.size()] = q_predicted.x(); 572 | x_predicted[O_r.size()+O_v.size()+1] = q_predicted.y(); 573 | x_predicted[O_r.size()+O_v.size()+2] = q_predicted.z(); 574 | x_predicted[O_r.size()+O_v.size()+3] = q_predicted.w(); 575 | 576 | // predict foot position and biases 577 | x_predicted.segment(dim_rvq_states, dim_states - dim_rvq_states) = x_prior.segment(dim_rvq_states, dim_states - dim_rvq_states); 578 | 579 | // Set variables to predicted 580 | setStateVariablesToPredicted(); 581 | } 582 | 583 | void EKF_RotellaEstimator::getMatrix_L_c(const dynacore::Quaternion & q_in, dynacore::Matrix & L_c_mat){ 584 | dynacore::Matrix C_mat = q_in.toRotationMatrix(); 585 | L_c_mat = dynacore::Matrix::Zero(dim_error_states, dim_process_errors); 586 | 587 | // L_c_mat.block(O_r.size(), 0, 3, 3) = -C_mat.transpose(); 588 | // L_c_mat.block(O_r.size() + O_v.size(), f_imu.size(), 3, 3) = -dynacore::Matrix::Identity(3,3); 589 | // L_c_mat.block(O_r.size() + O_v.size() + 3, f_imu.size() + omega_imu.size(), 3,3) = C_mat.transpose(); 590 | // L_c_mat.block(O_r.size() + O_v.size() + 3 + O_p_l.size(), f_imu.size() + omega_imu.size() + O_p_l.size(), 3, 3) = C_mat.transpose(); 591 | // L_c_mat.block(O_r.size() + O_v.size() + 3 + O_p_l.size() + O_p_r.size(), f_imu.size() + omega_imu.size() + O_p_l.size() + O_p_r.size(), 3, 3) = dynacore::Matrix::Identity(3,3); 592 | // L_c_mat.block(O_r.size() + O_v.size() + 3 + O_p_l.size() + O_p_r.size() + B_bw.size(), 593 | // f_imu.size() + omega_imu.size() + O_p_l.size() + O_p_r.size() + B_bw.size(), 3, 3) = dynacore::Matrix::Identity(3,3); 594 | 595 | L_c_mat.block(3, 0, 3, 3) = -C_mat.transpose(); 596 | L_c_mat.block(6, 3, 3, 3) = -dynacore::Matrix::Identity(3,3); 597 | L_c_mat.block(9, 6, 3,3) = C_mat.transpose(); 598 | L_c_mat.block(12, 9, 3, 3) = C_mat.transpose(); 599 | L_c_mat.block(15, 12, 3, 3) = dynacore::Matrix::Identity(3,3); 600 | L_c_mat.block(18, 15, 3, 3) = dynacore::Matrix::Identity(3,3); 601 | 602 | 603 | } 604 | 605 | void EKF_RotellaEstimator::getMatrix_Q(dynacore::Matrix & Q_mat){ 606 | Q_mat = dynacore::Matrix::Zero(dim_process_errors, dim_process_errors); 607 | Q_mat.block(0, 0, 3, 3) = wf_intensity*dynacore::Matrix::Identity(3,3); 608 | Q_mat.block(3, 3, 3, 3) = ww_intensity*dynacore::Matrix::Identity(3,3); 609 | Q_mat.block(6, 6, 3, 3) = wp_l_intensity*dynacore::Matrix::Identity(3,3); 610 | Q_mat.block(9, 9, 3, 3) = wp_r_intensity*dynacore::Matrix::Identity(3,3); 611 | Q_mat.block(12, 12, 3, 3) = wbf_intensity*dynacore::Matrix::Identity(3,3); 612 | Q_mat.block(15, 15, 3, 3) = wbw_intensity*dynacore::Matrix::Identity(3,3); 613 | 614 | } 615 | 616 | void EKF_RotellaEstimator::covariancePredictionStep(){ 617 | // Prepare Covariance Prediction Step 618 | // Construct linearized error dynamics matrix, F_c 619 | F_c = dynacore::Matrix::Zero(dim_error_states, dim_error_states); 620 | // F_c.block(0, O_r.size(), 3, 3) = dynacore::Matrix::Identity(3, 3); 621 | // F_c.block(O_r.size(), O_r.size() + O_v.size(), 3, 3) = -C_rot.transpose()*getSkewSymmetricMatrix(f_imu_input); 622 | // F_c.block(O_r.size(), dim_rvq_states + O_p_l.size() + O_p_r.size(), 3, 3) = -C_rot.transpose(); 623 | // F_c.block(O_r.size() + O_v.size(), O_r.size() + O_v.size(), 3, 3) = -getSkewSymmetricMatrix(omega_imu_input); 624 | // F_c.block(O_r.size() + O_v.size(), dim_error_states - B_bw.size(), B_bw.size(), B_bw.size()) = -dynacore::Matrix::Identity(3,3); 625 | 626 | F_c.block(0, 3, 3, 3) = dynacore::Matrix::Identity(3, 3); 627 | F_c.block(3, 6, 3, 3) = -C_rot.transpose()*getSkewSymmetricMatrix(f_imu_input); 628 | F_c.block(3, 15, 3, 3) = -C_rot.transpose(); 629 | F_c.block(6, 6, 3, 3) = -getSkewSymmetricMatrix(omega_imu_input); 630 | F_c.block(6, 18, 3,3) = -dynacore::Matrix::Identity(3,3); 631 | // F_c.block(6, 18, 3,3) = O_q_B.toRotationMatrix(); //-dynacore::Matrix::Identity(3,3); 632 | 633 | // Discretized linear error dynamics 634 | F_k = dynacore::Matrix::Identity(F_c.rows(), F_c.cols()) + F_c*dt; 635 | 636 | // Construct Process noise Jacobian Matrix, L_c 637 | getMatrix_L_c(O_q_B, L_c); 638 | getMatrix_Q(Q_c); 639 | 640 | // Perform Covariance prediction step 641 | P_predicted = F_k*P_prior*F_k.transpose() + F_k*L_c*Q_c*L_c.transpose()*F_k.transpose()*dt; 642 | //P_predicted = L_c*Q_c*L_c.transpose()*dt*dt; 643 | } 644 | 645 | void EKF_RotellaEstimator::predictionStep(){ 646 | statePredictionStep(); 647 | covariancePredictionStep(); 648 | } 649 | 650 | void EKF_RotellaEstimator::getCurrentBodyFrameFootPosition(const int foot_link_id, dynacore::Vector & foot_pos_B){ 651 | // Find Foot body id 652 | unsigned int bodyid; 653 | switch(foot_link_id){ 654 | case mercury_link::leftFoot: 655 | bodyid = robot_model->GetBodyId("lfoot"); 656 | break; 657 | case mercury_link::rightFoot: 658 | bodyid = robot_model->GetBodyId("rfoot"); 659 | break; 660 | default: 661 | printf("[EKF_RotellaEstimator] Not a valid foot link id\n"); 662 | exit(0); 663 | } 664 | 665 | // Set parameters up 666 | dynacore::Vect3 Local_CoM = robot_model->mFixedBodies[ 667 | bodyid - robot_model->fixed_body_discriminator].mCenterOfMass; 668 | 669 | // Align body frame to the fixed frame 670 | dynacore::Vector config_copy = Q_config; 671 | config_copy.head(O_r.size()) = dynacore::Vector::Zero(3); 672 | config_copy[O_r.size()] = 0.0; 673 | config_copy[O_r.size()+1] = 0.0; 674 | config_copy[O_r.size()+2] = 0.0; 675 | config_copy[mercury::num_qdot] = 1.0; 676 | 677 | foot_pos_B = CalcBodyToBaseCoordinates(*robot_model, config_copy, bodyid, Local_CoM, true); 678 | } 679 | 680 | void EKF_RotellaEstimator::getBodyVelFromKinematics(dynacore::Vector & O_body_vel){ 681 | dynacore::Vect3 vel; 682 | dynacore::Vect3 zero; 683 | 684 | unsigned int bodyid; 685 | if (lf_contact && rf_contact){ 686 | bodyid = robot_model->GetBodyId("lfoot"); 687 | }else if (lf_contact){ 688 | bodyid = robot_model->GetBodyId("lfoot"); 689 | }else if (rf_contact){ 690 | bodyid = robot_model->GetBodyId("rfoot"); 691 | }else{ 692 | bodyid = robot_model->GetBodyId("lfoot"); 693 | } 694 | 695 | if(bodyid >=robot_model->fixed_body_discriminator){ 696 | zero = robot_model->mFixedBodies[bodyid - robot_model->fixed_body_discriminator].mCenterOfMass; 697 | } 698 | else{ 699 | zero = robot_model->mBodies[bodyid].mCenterOfMass; 700 | } 701 | 702 | dynacore::Vector config_copy = Q_config; 703 | 704 | config_copy.head(O_r.size()) = O_r; 705 | config_copy[O_r.size()] = O_q_B.x(); 706 | config_copy[O_r.size()+1] = O_q_B.y(); 707 | config_copy[O_r.size()+2] = O_q_B.z(); 708 | config_copy[mercury::num_qdot] = O_q_B.w(); 709 | 710 | dynacore::Vector config_dot_copy = Q_config_dot; 711 | for(int i(0); i<3; ++i){ 712 | config_dot_copy[i+3] = omega_imu_input[i]; 713 | } 714 | 715 | vel = CalcPointVelocity (*robot_model, config_copy, config_dot_copy, bodyid, zero, true); 716 | 717 | // Set body velocity to negative of foot velocity 718 | for(size_t i = 0; i < 3; i++){ 719 | O_body_vel[i] = -vel[i]; 720 | } 721 | } 722 | 723 | void EKF_RotellaEstimator::updateStep(){ 724 | // Construct Noise Matrix R 725 | R_c.block(0,0,6,6) = n_p*dynacore::Matrix::Identity(6,6); 726 | 727 | // Get body velocity estimate from kinematics 728 | getBodyVelFromKinematics(body_vel_kinematics); 729 | R_c.block(6,6,3,3) = n_v*dynacore::Matrix::Identity(3,3); 730 | 731 | R_k = R_c/dt; 732 | 733 | // Construct Discretized Observation Matrix 734 | H_k.block(0,0,3,3) = -C_rot; 735 | p_l_B = C_rot*(O_p_l - O_r); // the left foot position in body frame 736 | H_k.block(0,6,3,3) = getSkewSymmetricMatrix(p_l_B); 737 | H_k.block(0,9,3,3) = C_rot; 738 | 739 | H_k.block(3,0,3,3) = -C_rot; 740 | p_r_B = C_rot*(O_p_r - O_r); // the right foot position in body frame 741 | H_k.block(3,6,3,3) = getSkewSymmetricMatrix(p_r_B); 742 | H_k.block(3,12,3,3) = C_rot; 743 | 744 | H_k.block(6, 3, 3, 3) = dynacore::Matrix::Identity(3,3); 745 | 746 | //Compute the Kalman Gain 747 | S_k = H_k*P_predicted*H_k.transpose() + R_k; 748 | K_k = P_predicted*H_k.transpose()*(S_k.inverse()); 749 | 750 | // Perform Measurement Using Kinematics 751 | getCurrentBodyFrameFootPosition(mercury_link::leftFoot, z_lfoot_pos_B); 752 | getCurrentBodyFrameFootPosition(mercury_link::rightFoot, z_rfoot_pos_B); 753 | 754 | // Measurement error vector 755 | // y_vec = (z - h()) 756 | y_vec.head(O_p_l.size()) = z_lfoot_pos_B - p_l_B; 757 | y_vec.segment(O_p_l.size(), O_p_r.size()) = z_rfoot_pos_B - p_r_B; 758 | y_vec.tail(O_v.size()) = body_vel_kinematics - O_v; 759 | 760 | delta_x_posterior = K_k*y_vec; 761 | //delta_y = H_k*delta_x_posterior; 762 | 763 | // Update State 764 | updateStatePosterior(); 765 | 766 | // Update Covariance Matrix 767 | P_posterior = (dynacore::Matrix::Identity(dim_error_states, dim_error_states) - K_k*H_k)*P_predicted; 768 | P_prior = P_posterior; 769 | 770 | 771 | // Update Prior 772 | //x_prior = x_predicted; 773 | //P_prior = P_predicted; 774 | } 775 | 776 | void EKF_RotellaEstimator::updateStatePosterior(){ 777 | x_posterior = x_predicted; 778 | x_posterior.head(O_r.size() + O_v.size()) += delta_x_posterior.head(O_r.size() + O_v.size()); 779 | x_posterior.tail(O_p_l.size() + O_p_r.size() + B_bf.size() + B_bw.size()) += delta_x_posterior.tail(O_p_l.size() + O_p_r.size() + B_bf.size() + B_bw.size()); 780 | 781 | // Prepare quaternion update 782 | dynacore::Quaternion q_prediction, q_change, q_posterior; 783 | q_prediction.x() = x_predicted[O_r.size()+O_v.size()]; 784 | q_prediction.y() = x_predicted[O_r.size()+O_v.size()+1]; 785 | q_prediction.z() = x_predicted[O_r.size()+O_v.size()+2]; 786 | q_prediction.w() = x_predicted[O_r.size()+O_v.size()+3]; 787 | 788 | // Convert delta phi change to quaternion 789 | delta_phi[0] = delta_x_posterior[O_r.size() + O_v.size()]; 790 | delta_phi[1] = delta_x_posterior[O_r.size() + O_v.size() + 1]; 791 | delta_phi[2] = delta_x_posterior[O_r.size() + O_v.size() + 2]; 792 | dynacore::convert(delta_phi, q_change); 793 | 794 | // Perform Quaternion update 795 | q_posterior = dynacore::QuatMultiply(q_change, q_prediction); 796 | q_posterior = q_prediction; 797 | 798 | x_posterior[O_r.size()+O_v.size()] = q_posterior.x(); 799 | x_posterior[O_r.size()+O_v.size()+1] = q_posterior.y(); 800 | x_posterior[O_r.size()+O_v.size()+2] = q_posterior.z(); 801 | x_posterior[O_r.size()+O_v.size()+3] = q_posterior.w(); 802 | 803 | x_prior = x_posterior; 804 | } 805 | 806 | 807 | 808 | void EKF_RotellaEstimator::doFilterCalculations(){ 809 | predictionStep(); 810 | updateStep(); 811 | } 812 | 813 | 814 | 815 | void EKF_RotellaEstimator::showPrintOutStatements(){ 816 | //printf("\n"); 817 | // printf("[EKF Rotella Estimator]\n"); 818 | // dynacore::pretty_print(f_imu, std::cout, "body frame f_imu"); 819 | // dynacore::pretty_print(omega_imu, std::cout, "body frame omega_imu"); 820 | // dynacore::Vector a_o = (C_rot.transpose()*f_imu_input + gravity_vec); 821 | // dynacore::pretty_print(O_q_B, std::cout, "O_q_B"); 822 | // dynacore::pretty_print(C_rot, std::cout, "C_rot"); 823 | // dynacore::pretty_print(a_o, std::cout, "inertial frame f_imu"); 824 | 825 | // dynacore::pretty_print(omega_imu, std::cout, "body frame angular velocity"); 826 | 827 | // dynacore::pretty_print(f_imu_input, std::cout, "f_imu_input"); 828 | // dynacore::pretty_print(omega_imu_input, std::cout, "omega_imu_input"); 829 | 830 | // dynacore::pretty_print(x_prior, std::cout, "x_prior"); 831 | // dynacore::pretty_print(x_predicted, std::cout, "x_predicted"); 832 | // dynacore::pretty_print(y_vec, std::cout, "y_vec"); 833 | // dynacore::pretty_print(delta_x_posterior, std::cout, "delta_x_posterior"); 834 | 835 | //dynacore::pretty_print(R_k, std::cout, "R_k"); 836 | // dynacore::pretty_print(delta_phi, std::cout, "delta_phi"); 837 | // dynacore::pretty_print(x_posterior, std::cout, "x_posterior"); 838 | 839 | 840 | // dynacore::pretty_print(F_c, std::cout, "F_c"); 841 | //dynacore::pretty_print(F_k, std::cout, "F_k"); 842 | // dynacore::pretty_print(L_c, std::cout, "L_c"); 843 | // dynacore::pretty_print(Q_c, std::cout, "Q_c"); 844 | // dynacore::pretty_print(P_prior, std::cout, "P_prior"); 845 | // dynacore::pretty_print(H_k, std::cout, "H_k"); 846 | 847 | // dynacore::pretty_print(K_k, std::cout, "K_k"); 848 | 849 | // dynacore::pretty_print(z_lfoot_pos_B, std::cout, "z_lfoot"); 850 | // dynacore::pretty_print(z_rfoot_pos_B, std::cout, "z_rfoot"); 851 | //dynacore::pretty_print(y_vec, std::cout, "y_vec"); 852 | 853 | 854 | // printf("Left Foot contact = %d \n", lf_contact); 855 | // printf("Right Foot contact = %d \n", rf_contact); 856 | // dynacore::pretty_print(Q_config, std::cout, "Q_config"); 857 | } 858 | -------------------------------------------------------------------------------- /EKF_RotellaEstimator.hpp: -------------------------------------------------------------------------------- 1 | #ifndef EKF_ROTELLA_ESTIMATOR 2 | #define EKF_ROTELLA_ESTIMATOR 3 | 4 | #include "EKF_PoseEstimator.hpp" 5 | #include 6 | #include 7 | 8 | 9 | /* This estimator is based on: 10 | 11 | Rotella, Nicholas, et al. 12 | "State estimation for a humanoid robot." 13 | Intelligent Robots and Systems (IROS 2014), 14 | 2014 IEEE/RSJ International Conference on. IEEE, 2014. 15 | 16 | and 17 | 18 | Bloesch, Michael, et al. 19 | "State estimation for legged robots-consistent fusion of leg kinematics and IMU." 20 | Robotics 17 (2013): 17-24. 21 | 22 | */ 23 | 24 | class EKF_RotellaEstimator :public EKF_PoseEstimator{ 25 | public: 26 | EKF_RotellaEstimator(); 27 | ~EKF_RotellaEstimator(); 28 | virtual void setSensorData(const std::vector & acc, 29 | const std::vector & acc_inc, 30 | const std::vector & ang_vel, 31 | const bool & left_foot_contact, 32 | const bool & right_foot_contact, 33 | const dynacore::Vector & joint_values); 34 | 35 | virtual void setSensorData(const std::vector & acc, 36 | const std::vector & acc_inc, 37 | const std::vector & ang_vel, 38 | const bool left_foot_contact, 39 | const bool right_foot_contact, 40 | const dynacore::Vector & joint_values, 41 | const dynacore::Vector & joint_velocity_values); 42 | 43 | virtual void EstimatorInitialization(const dynacore::Quaternion & initial_global_orientation, 44 | const std::vector & initial_imu_acc, 45 | const std::vector & initial_imu_ang_vel); 46 | 47 | virtual void resetFilter(); 48 | 49 | dynacore::Matrix getSkewSymmetricMatrix(dynacore::Vector & vec_in); 50 | 51 | 52 | void handleFootContacts(); 53 | void computeNewFootLocations(const int foot_link_id); 54 | 55 | void setStateVariablesToPrior(); 56 | void setStateVariablesToPredicted(); 57 | 58 | void showPrintOutStatements(); 59 | 60 | void getMatrix_L_c(const dynacore::Quaternion & q_in, dynacore::Matrix & L_c_mat); 61 | void getMatrix_Q(dynacore::Matrix & Q_mat); 62 | void statePredictionStep(); 63 | void covariancePredictionStep(); 64 | 65 | void getCurrentBodyFrameFootPosition(const int foot_link_id, dynacore::Vector & foot_pos_B); 66 | 67 | void getBodyVelFromKinematics(dynacore::Vector & O_body_vel); 68 | 69 | void updateStatePosterior(); 70 | 71 | void predictionStep(); 72 | void updateStep(); 73 | void doFilterCalculations(); 74 | 75 | 76 | 77 | void calibrateOrientationFromGravity(); 78 | 79 | protected: 80 | dynacore::Vector O_p_l; // global left foot position 81 | dynacore::Vector O_p_r; // global right foot position 82 | 83 | dynacore::Vector B_bf; // imu frame acceleration bias 84 | dynacore::Vector B_bw; // imu frame angular velocity bias 85 | 86 | dynacore::Vector f_imu; // imu frame acceleration 87 | dynacore::Vector omega_imu; // imu frame angular velocity 88 | 89 | dynacore::Vector f_imu_input; // imu frame acceleration 90 | dynacore::Vector omega_imu_input; // imu frame angular velocity 91 | 92 | bool initial_foot_contact; // checks whether an initial foot contact at startup has occured for the first time. 93 | 94 | bool lf_contact; // value of left foot contact 95 | bool rf_contact; // value of right foot contact 96 | 97 | bool prev_lf_contact; // previous value of the left foot contact 98 | bool prev_rf_contact; // previous value of the right foot contact 99 | 100 | RigidBodyDynamics::Model* robot_model; 101 | 102 | dynacore::Vector Q_config; // configuration of the robot_model 103 | dynacore::Vector Q_config_dot; // configuration velocity of the robot model 104 | 105 | 106 | int count; 107 | // EKF Variables----------------------------------- 108 | double local_gravity; 109 | dynacore::Vector gravity_vec; 110 | double dt; 111 | 112 | int dim_states; 113 | int dim_rvq_states; 114 | int dim_process_errors; 115 | int dim_error_states; 116 | int dim_obs; 117 | int dim_inputs; 118 | 119 | dynacore::Matrix C_rot; // rotation matrix from inerital frame to body frame. 120 | 121 | dynacore::Matrix F_c; // contiouous prediction Jacobian. aka: f_x (linearized error state dynamics) 122 | dynacore::Matrix L_c; // continuous process error Jacobian 123 | dynacore::Matrix Q_c; // continuous process noise 124 | double wf_intensity; // imu process noise intensity 125 | double ww_intensity; // angular velocity noise intensity 126 | double wp_l_intensity; // left foot location noise intensity 127 | double wp_r_intensity; // right foot location noise intensity 128 | 129 | double wp_intensity_default; // default foot location noise intensity 130 | double wp_intensity_unknown; // noise intensity when there is no foot contact 131 | 132 | double wbf_intensity; // imu bias intensity 133 | double wbw_intensity; // angular velocity bias intensity 134 | 135 | dynacore::Matrix F_k; // discretized error state prediction matrix 136 | dynacore::Matrix H_k; // discretized error state observation matrix 137 | 138 | dynacore::Matrix R_c; // Measurement noise covariance matrix 139 | dynacore::Matrix R_k; // Discretized measurement noise covariance matrix 140 | double n_p; // Measurement noise intensity 141 | 142 | 143 | double n_v_default; // default noise intensity of body velocity measurement 144 | double n_v_unknown; // unknown noise intensity from body velocity measurement 145 | double n_v; // Body velocity from kinematics measurement noise intensity 146 | dynacore::Vector body_vel_kinematics; 147 | 148 | 149 | dynacore::Matrix S_k; // Innovation (residual) covariance 150 | dynacore::Matrix K_k; // Kalman gain 151 | 152 | dynacore::Matrix P_prior; // Prior covariance matrix 153 | dynacore::Matrix P_predicted; // Predicted covariance matrix 154 | dynacore::Matrix P_posterior; // Posterior covariance matrix 155 | 156 | dynacore::Vector x_prior; // Prior EKF States 157 | dynacore::Vector x_predicted; // Predicted EKF States 158 | dynacore::Vector x_posterior; // Posterior EKF States 159 | 160 | dynacore::Vector delta_x_prior; // Prior error states 161 | dynacore::Vector delta_x_posterior; // Posterier error states 162 | dynacore::Vector delta_y; // prediction and measurement differences 163 | 164 | dynacore::Vector z_lfoot_pos_B; // Measured body frame left foot position 165 | dynacore::Vector z_rfoot_pos_B; // Measured body frame right foot position 166 | 167 | dynacore::Vector p_l_B; // Body frame left foot position 168 | dynacore::Vector p_r_B; // Body frame right foot position 169 | 170 | dynacore::Vector y_vec; // prediction and measurement differences 171 | 172 | dynacore::Vect3 delta_phi; 173 | 174 | // Initialize Orientation Calibration Variables 175 | double theta_x; 176 | double theta_y; 177 | double gravity_mag; 178 | double roll_value_comp; 179 | double pitch_value_comp; 180 | 181 | dynacore::Vect3 g_B; 182 | dynacore::Vect3 g_B_local_vec; // rotated gravity direction 183 | dynacore::Quaternion Oq_B_init; // initial quaternion of the body frame w.r.t fixed frame 184 | dynacore::Matrix OR_B_init; // initial Rot matrix of body w.r.t fixed frame 185 | dynacore::Quaternion q_world_Rx; 186 | dynacore::Quaternion q_world_Ry; 187 | 188 | double bias_lp_frequency_cutoff; 189 | digital_lp_filter* x_bias_low_pass_filter; 190 | digital_lp_filter* y_bias_low_pass_filter; 191 | digital_lp_filter* z_bias_low_pass_filter; 192 | double x_acc_bias; 193 | double y_acc_bias; 194 | double z_acc_bias; 195 | 196 | }; 197 | 198 | 199 | #endif 200 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | This is a code dump of a previous implementation of the following papers 2 | 3 | ```` 4 | Rotella, Nicholas, et al. 5 | "State estimation for a humanoid robot." 6 | Intelligent Robots and Systems (IROS 2014), 7 | 2014 IEEE/RSJ International Conference on. IEEE, 2014. 8 | 9 | Bloesch, Michael, et al. 10 | "State estimation for legged robots-consistent fusion of leg kinematics and IMU." 11 | Robotics 17 (2013): 17-24. 12 | ```` 13 | 14 | It has dependencies which I have not included. But it should be readable. --------------------------------------------------------------------------------