def testNonLinearStateUpdate(): """ Test filter with a non-linear state update """ SAMPLES = 1000 def stateUpdate(state, control): state = np.asarray(state) newState = np.empty_like(state) newState[0] = state[0] + 0.001 * state[1]**2 newState[1] = state[1] + 0.01 return newState measurementFunc = lambda state: state initialState = np.matrix('0;0') initialCovariance = np.matrix('1,0.5;0.5,1') processCovariance = np.matrix('0.01,0.01;0.01,0.01') measurementCovariance = np.matrix('0.25,0;0,0.25') ukf = UnscentedKalmanFilter(stateUpdate, measurementFunc, initialState, initialCovariance, processCovariance, measurementCovariance) trueStates = np.empty((2, SAMPLES)) trueStates[:, 0] = np.array([1, 2]) measurements = np.empty_like(trueStates) estimates = np.empty_like(trueStates) estimateCovariances = np.empty((SAMPLES, 2, 2)) for i in range(1, SAMPLES): trueStates[:, i] = stateUpdate(trueStates[:, i - 1], 0) measurements[:, i] = trueStates[:, i] + np.random.normal( loc=0, scale=0.5, size=2) ukf.update(measurements[:, i:i + 1]) estimates[:, i:i + 1] = ukf.state estimateCovariances[i] = ukf.stateCovariance # Allow ten samples for estimates to start settling errors = trueStates[:, 10:] - estimates[:, 10:] assert np.mean(errors[0]**2) < 0.1 assert np.mean(errors[1]**2) < 0.1 assert np.var(errors[0]) < measurementCovariance[0, 0] assert np.var(errors[1]) < measurementCovariance[1, 1]
def testNonLinearStateUpdate(): """ Test filter with a non-linear state update """ SAMPLES = 1000 def stateUpdate(state, control): state = np.asarray(state) newState = np.empty_like(state) newState[0] = state[0] + 0.001*state[1]**2 newState[1] = state[1] + 0.01 return newState measurementFunc = lambda state: state initialState = np.matrix('0;0') initialCovariance = np.matrix('1,0.5;0.5,1') processCovariance = np.matrix('0.01,0.01;0.01,0.01') measurementCovariance = np.matrix('0.25,0;0,0.25') ukf = UnscentedKalmanFilter(stateUpdate, measurementFunc, initialState, initialCovariance, processCovariance, measurementCovariance) trueStates = np.empty((2,SAMPLES)) trueStates[:,0] = np.array([1,2]) measurements = np.empty_like(trueStates) estimates = np.empty_like(trueStates) estimateCovariances = np.empty((SAMPLES,2,2)) for i in xrange(1,SAMPLES): trueStates[:,i] = stateUpdate(trueStates[:,i-1],0) measurements[:,i] = trueStates[:,i] + np.random.normal(loc=0, scale=0.5, size=2) ukf.update(measurements[:,i:i+1]) estimates[:,i:i+1] = ukf.state estimateCovariances[i] = ukf.stateCovariance # Allow ten samples for estimates to start settling errors = trueStates[:,10:]-estimates[:,10:] assert np.mean(errors[0]**2) < 0.1 assert np.mean(errors[1]**2) < 0.1 assert np.var(errors[0]) < measurementCovariance[0,0] assert np.var(errors[1]) < measurementCovariance[1,1]
def testNonLinearMeasurement(): """ Tests filter estimating constants with a non-linear measurement function """ SAMPLES = 1000 stateUpdate = lambda state, control: state def measurementFunc(state): state = np.asarray(state) newState = np.empty_like(state) newState[0] = state[0]**2 newState[1] = state[1]**3 return newState initialState = np.matrix('0.1;0.1') initialCovariance = np.matrix('1,0;0,1') processCovariance = np.matrix('0.001,0;0,0.001') measurementCovariance = np.matrix('0.01,0;0,0.01') ukf = UnscentedKalmanFilter(stateUpdate, measurementFunc, initialState, initialCovariance, processCovariance, measurementCovariance) trueStates = np.empty((2,SAMPLES)) trueStates.fill(2) measurements = measurementFunc(trueStates) + np.random.normal(loc=0, scale=0.1, size=(2,SAMPLES)) estimates = np.empty_like(trueStates) estimateCovariances = np.empty((SAMPLES,2,2)) for i in xrange(SAMPLES): ukf.update(measurements[:,i:i+1]) estimates[:,i:i+1] = ukf.state estimateCovariances[i] = ukf.stateCovariance # Allow ten samples for estimates to start settling errors = trueStates[:,10:]-estimates[:,10:] assert np.mean(errors[0]**2) < 0.01 assert np.mean(errors[1]**2) < 0.01 assert np.var(errors[0]) < measurementCovariance[0,0] assert np.var(errors[1]) < measurementCovariance[1,1]
def testNonLinearMeasurement(): """ Tests filter estimating constants with a non-linear measurement function """ SAMPLES = 1000 stateUpdate = lambda state, control: state def measurementFunc(state): state = np.asarray(state) newState = np.empty_like(state) newState[0] = state[0]**2 newState[1] = state[1]**3 return newState initialState = np.matrix('0.1;0.1') initialCovariance = np.matrix('1,0;0,1') processCovariance = np.matrix('0.001,0;0,0.001') measurementCovariance = np.matrix('0.01,0;0,0.01') ukf = UnscentedKalmanFilter(stateUpdate, measurementFunc, initialState, initialCovariance, processCovariance, measurementCovariance) trueStates = np.empty((2, SAMPLES)) trueStates.fill(2) measurements = measurementFunc(trueStates) + np.random.normal( loc=0, scale=0.1, size=(2, SAMPLES)) estimates = np.empty_like(trueStates) estimateCovariances = np.empty((SAMPLES, 2, 2)) for i in range(SAMPLES): ukf.update(measurements[:, i:i + 1]) estimates[:, i:i + 1] = ukf.state estimateCovariances[i] = ukf.stateCovariance # Allow ten samples for estimates to start settling errors = trueStates[:, 10:] - estimates[:, 10:] assert np.mean(errors[0]**2) < 0.01 assert np.mean(errors[1]**2) < 0.01 assert np.var(errors[0]) < measurementCovariance[0, 0] assert np.var(errors[1]) < measurementCovariance[1, 1]