Kalman Filter Update

The Kalman filter is working! Threshold still works just different more to come on behavior analysis