Adding noise covariances to KF and initial pose

[howard-beck]

next step is to make it calculate stuff but that's scary so we're stopping here :)