int main(int argc, char *argv[]) { for(int i = 1; i < argc; i++) { HuffmanEncoder hEnc(argv[i]); hEnc.Run(); } return 1; }
// 6. Total measurement update void Ekf::measurementUpdateEncoders(Vector2i z, Vector2i zPre, double dt){ // z is encL and encR // Determine h and H Vector2d h; h = hEnc(state_, b_, tpmRight_, tpmLeft_, zPre(0), zPre(1), dt); Matrix26 H; H = HEnc(state_, b_, tpmRight_, tpmLeft_, dt); // Find Kalman Gain Matrix62 K; K = KEnc(cov_, H, REnc_); // Find new state state_ = stateUpdateEnc(state_, K, z, h); // Find new covariance cov_ = covUpdateEnc(cov_, K, H); cov_ = zeroOutBiasXYThetaCov(cov_); }