add NHC constraint in EKF

[improve100]

Hello, considering the vehicle model, I want to add NHC constraint, assuming y = 0 x = 0 Is the update modified to

double roll = state(StateMemberRoll);
double pitch = state(StateMemberPitch);
double yaw = state(StateMemberYaw);
double xVel = state(StateMemberVx);
double yVel = state(StateMemberVy);
double zVel = state(StateMemberVz);

Eigen::Quaterniond cnb_q;
cnb_q = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
         * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
         * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
Eigen::Matrix3d Cnb = cnb_q.toRotationMatrix();
Eigen::Vector3d vins = Cnb * Eigen::Vector3d(xVel,yVel,zVel);
Eigen::Matrix3d vins_skew_matrix;
vins_skew_matrix << 0., -vins(2), vins(1), vins(2), 0., -vins(0), -vins(1), vins(0), 0.;
Eigen::Vector3d dv = Eigen::Vector3d(xVel_new,yVel_new,zVel_new) - Eigen::Vector3d(xVel,yVel,zVel);
Eigen::Vector3d dr = (Cnb.transpose()*vins_skew_matrix).inverse()*(-dv);

std::cout<<"deltaRotation roll pitch:"<<dr.transpose()<<std::endl;