[Fix] Compute Kalman gain in update_iterated_dyn_share_diagonal when n > dof_Measurement

[Daehan2Lee]

Summary

This PR addresses a critical issue in the update_iterated_dyn_share_diagonal function of esekfom.hpp.
When the number of state dimensions (n) exceeds the measurement dimension (dof_Measurement), the
Kalman gain computation was disabled (the intended code was commented out). As a result, in scenarios
with very few measurements, the filter update was performed incorrectly, leading to divergence.

Changes

• Original Code:

  The code in the if(n > dof_Measurement) branch originally only created an h_x_cur matrix by copying h_x_
  into the top-left corner:

  if(n > dof_Measurement)
  {
      std::printf("\n\n\n Too few measurement, n > dof_Measurement!!!\n\n\n");
      //#ifdef USE_sparse
      //Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_temp = h_x * P_ * h_x.transpose();
      //spMt R_temp = h_v * R_ * h_v.transpose();
      //K_temp += R_temp;
      Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);
      h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_;
      //Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_ = P_ * h_x_cur.transpose() * (h_x_cur * P_ * h_x_cur.transpose()/R + Eigen::Matrix<double, Dynamic, Dynamic>::Identity(dof_Measurement, dof_Measurement)).inverse()/R;
      //K_h = K_ * dyn_share.h;
      //K_x = K_ * h_x_cur;
      //#else
      //  K_ = P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse();
      //#endif
  }
  

• Modified Code:

  The fix extends the available measurement Jacobian to a full measurement Jacobian (H_full)
  by padding with zeros. Additionally, both operands in the addition are converted to dense matrices,
  which is necessary because Eigen's lazy-evaluated product expressions and DiagonalWrapper do not
  support operator+ directly. The updated code is as follows:

  if(n > dof_Measurement)
  {
      std::printf("\n\n\n Too few measurement, n > dof_Measurement!!!\n\n\n");
      Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> H_full =
          Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);
      H_full.topLeftCorner(dof_Measurement, 12) = h_x_;
      
      // Convert lazy-evaluated product and diagonal matrix to dense matrices:
      Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> S =
          (H_full * P_ * H_full.transpose()).eval() + dyn_share.R.asDiagonal().toDenseMatrix();
      
      // Compute the Kalman gain:
      Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_ =
          P_ * H_full.transpose() * S.inverse();
      
      K_h = K_ * dyn_share.h;
      K_x = K_ * H_full;
  }
  

  This ensures that the operands of the addition are both dense matrices, resolving the operator+
  error and enabling a valid Kalman gain computation even when measurements are few.

Testing

• I have verified that the code now compiles without errors.
• In scenarios where the measurement dimension is low (n > dof_Measurement), the Kalman gain is
  computed correctly, and the filter update behaves as expected without divergence.

Notes

Using .eval() and .toDenseMatrix() forces the lazy Eigen expressions to be evaluated into standard dense
matrices. This conversion is necessary because the DiagonalWrapper (returned by dyn_share.R.asDiagonal())
cannot be directly added to an unevaluated product expression. This change ensures numerical consistency
and proper filter updates even in low-measurement cases.