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_);
}
