Merged from OptimalKalmanFilter