Kalman begun

There's still a lot of the PFAgent code in it that needs to be torn out,
but I've set up a lot of matrices, and the update functions. The agent
doesn't use them yet