Undeclared variable reference bug.

A variable that didn't exist was being referenced. Corrected the code-
the dimension for the second dimension of the array is always 1.
Faster UKF.

Used numpy broadcasting rules to speed up the UKF computations.
This needs to be moved to the scaled UKF.

Also some additional tests are written (not related to the speed up)