added Kalman Filter functions
started with Kalman Filtering
non-invertible state covariance matrix
implemented kalman filter