Function Jplus2

uav_simulator/Utils/pose_utils/src/pose_utils.cpp:255–283

}

mat Jplus2(const colvec& X1, const colvec& X2) {
  colvec X3 = pose_update(X1, X2);
  mat R1 = ypr_to_R(X1.rows(3, 5));
  mat R2 = ypr_to_R(X2.rows(3, 5));
  mat R3 = ypr_to_R(X3.rows(3, 5));
  colvec o1 = R1.col(1);
  colvec a1 = R1.col(2);
  colvec o2 = R2.col(1);
  colvec a2 = R2.col(2);

  mat Z = zeros<mat>(3, 3);

  double K2arr[9] = { cos(X2(5 - 1)) * cos(X3(6 - 1) - X2(6 - 1)) / cos(X3(5 - 1)),
                      sin(X3(6 - 1) - X2(6 - 1)) / cos(X3(5 - 1)),
                      0,
                      -cos(X2(5 - 1)) * sin(X3(6 - 1) - X2(6 - 1)),
                      cos(X3(6 - 1) - X2(6 - 1)),
                      0,
                      (a1(1 - 1) * cos(X3(4 - 1)) + a1(2 - 1) * sin(X3(4 - 1))) / cos(X3(5 - 1)),
                      sin(X3(5 - 1)) * sin(X3(6 - 1) - X2(6 - 1)) / cos(X3(5 - 1)),
                      1 };
  mat K2(3, 3);
  for (int i = 0; i < 9; i++)
    K2(i) = K2arr[i];
  K2 = trans(K2);

  mat J2 = join_cols(join_rows(R1, Z), join_rows(Z, K2));
  return J2;
}

// For IMU EKF ----------------------