ResetState()问题

[improve100]

// 只将状态量置零 void ESKFQK::ResetState() { X_.setZero(); 这一步已经把X_设置为0了，后面的G相当于把对应更新位置的P值置为0吗？ // P = G'PG'^T Eigen::Matrix<double,DIM_STATE,DIMSTATE> G; G.setIdentity(); G.block<3,3>(6,6) -= BuildSkewMatrix(0.5*X.block<3,1>(INDEX_STATEORI, 0)); P = GP_G.transpose(); }

[Shelfcol]

这里确实有点问题，我当时直接根据论文(paper里的)公式(181,182)写的，P_的更新应该在X_置0之前。

[improve100]

我想添加一个2D模式可以这样更新吗？ void forceTwoD() { Eigen::Matrix<double, 9, 1> residual; residual.setZero(); residual(2) = -pose(2); residual(5) = -velocity(2);

Eigen::Vector3d angle = computeAngles(q_);
residual(6) = -angle(0);
residual(7) = -angle(1);

Eigen::Matrix<double, 9, 19> Hx_;
Hx_.setZero();

Hx_(2,2) = 1;
Hx_(5,5) = 1;
Hx_(6,6) = 1;
Hx_(7,7) = 1;

Eigen::Matrix<double, 19, 15> X_dx_;
X_dx_.setZero();
X_dx_.block<6,6>(0,0) = Eigen::Matrix<double,6,6>::Identity();
X_dx_.block<6,6>(10,9) = Eigen::Matrix<double,6,6>::Identity();

Eigen::Quaterniond q(kf_ptr_->state_ptr_->r_GI);
X_dx_.block<4,3>(6,6) = 0.5*(Eigen::Matrix<double,4,3>()<<-q.x(),-q.y(),-q.z(),
                             q.w(),-q.z(),q.y(),
                             q.z(),q.w(),-q.x(),
                             -q.y(),q.x(),q.w()).finished();

Eigen::Matrix<double, 9, 18> H_;
H_.setZero();
H_ = Hx_ * X_dx_;

Eigen::Matrix<double,9,9> V_;
// V.setZero();
// V = Eigen::Matrix<double,9,9>::Identity() * std::numeric_limits<double>::max();
V_ = Eigen::Matrix<double,9,9>::Identity();
V_(2,2) = 1e-6;
V_(5,5) = 1e-6;
V_(6,6) = 1e-6;
V_(7,7) = 1e-6;

K_ = P_ * H_.transpose() * (H_ * P_ * H_.transpose() +V_).inverse(); // kalman增益
X_ = K_*(residual);

P_ = (TypeMatrixP::Identity() - K_ * H_) * P_*(TypeMatrixP::Identity() - K_ * H_).transpose()+K_*V_*K_.transpose();
return true;

}

[Shelfcol]

看你这个意思应该是想加Pz和Vz,还有roll和pitch角观测？你在算residual的时候residual(2) = -pose_(2)，以及你这里的residual(6)使用的是nominal state来计算的。残差应该是观测-观测矩阵*先验，不是nominal state 取负。你可以看一下公式(171)的残差计算。（请看我的repo里面的paper，以便对应公式）

[improve100]

谢谢。我是把观测矩阵直接当成单位矩阵了。因为是平面运动z=0,vz=0,roll=pitch=0. 所以把状态量直接取负作为残差了。

[Shelfcol]

那你其他维度的观测为什么不求残差呢

[Shelfcol]

你的意思是2D的xy yaw都没有观测吗？，只使用2D约束，也就是 z, Vz, roll ,pitch都因该等于0

[improve100]

是的。平面运动z=0,vz=0,roll=pitch=0. 其他维度x,y,vx,vy,yaw是不可观测的。我这里把2d约束当成一个虚拟的传感器来定时更新。