状态和面向对象编程——9.一直在移动

一直在移动

无人驾驶车会不断监视自身状态。所以，移动和定位必须平行进行。

如果我们使用卡尔曼滤波器进行定位，这意味着，随着车辆的移动，卡尔曼滤波器必须不断提供新的状态估计值。这可以保证车辆始终 知道它的位置。

一直在预测状态

在下面的代码中，给出了一个 predict_state 函数，它接受当前状态和时间变化 dt，并返回新的状态估计（基于恒定速度模型）。

你可以反复使用这个函数，来查找 5 个不同的时间点的预测状态（predicted_state）：

• 初始状态
• 2 秒后的预测状态
• 再过 3 秒后的预测状态
• 再过 1 秒后的预测状态
• 再过 4 秒后的预测状态

前三个状态已经在代码中给出。

from functions import predict_state

# predict_state takes in a state and a change in time, dt
# So, a call might look like: new_state = predict_state(old_state, 2)

# The car starts at position = 0, going 60 m/s
# The initial state:
initial_state = [10, 60]

# After 2 seconds:
state_est1 = predict_state(initial_state, 2)

# 3 more seconds after the first estimated state
state_est2 = predict_state(state_est1, 3)

## TODO: Use the predict_state function 
## and the above variables to calculate the following states
## (And change their value from 0 to the correct state)

## Then, click Test Run to see your results!

## 1 more second after the second state estimate
state_est3 = predict_state(state_est2,1)

## 4 more seconds after the third estimated state
state_est4 = predict_state(state_est3,4)

#---- predict state function --#
def predict_state(state, dt):
    # Assumes a valid state had been passed in
    # Assumes a constant velocity model
    x = state[0]
    new_x = x+state[1]*dt
    
    # Create and return the new, predicted state
    predicted_state = [new_x, state[1]]
    return predicted_state