Function Jplus1

uav_simulator/Utils/pose_utils/src/pose_utils.cpp:212–253

// For Pose EKF ----------------------

mat Jplus1(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 I = eye<mat>(3, 3);
  mat Z = zeros<mat>(3, 3);

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

  double K1arr[9] = { 1,
                      sin(X3(5 - 1)) * sin(X3(4 - 1) - X1(4 - 1)) / cos(X3(5 - 1)),
                      (o2(1 - 1) * sin(X3(6 - 1)) + a2(1 - 1) * cos(X3(6 - 1))) / cos(X3(5 - 1)),
                      0,
                      cos(X3(4 - 1) - X1(4 - 1)),
                      -cos(X1(5 - 1)) * sin(X3(4 - 1) - X1(4 - 1)),
                      0,
                      sin(X3(4 - 1) - X1(4 - 1)) / cos(X3(5 - 1)),
                      cos(X1(5 - 1)) * cos(X3(4 - 1) - X1(4 - 1)) / cos(X3(5 - 1)) };
  mat K1(3, 3);
  for (int i = 0; i < 9; i++)
    K1(i) = K1arr[i];
  K1 = trans(K1);

  mat J1 = join_cols(join_rows(I, M), join_rows(Z, K1));
  return J1;
}

mat Jplus2(const colvec& X1, const colvec& X2) {
  colvec X3 = pose_update(X1, X2);