定位步驟
卡爾曼濾波器
回顧一下卡爾曼用語定位的步驟：
- 初步預測
下面是一個例子，我們知道我們的車在這條單車道上，但我們不知道它的確切位置。
- 測量更新 感知周圍的世界稱之爲測量更新。收集有關汽車周圍環境的更多信息並改進我們的預測。 比如：測量的停車標誌前兩個網格單元，測量並不完美，但我們對汽車的位置有了更好的瞭解。
測量更新步驟
- 預測(時間更新) 下一步是移動，也稱之爲時間更新或預測步驟；我們根據對速度和當前位置的瞭解，預測汽車的移動位置。 比如：將概率分佈向右移動一個單元格
預測步驟
- 循環重複 已經形成了對汽車位置的新估計，卡爾曼濾波器簡單的重複感應和移動(測量和預測)，以便在汽車移動時對汽車進行定位！
卡爾曼濾波器可以結合不準確的傳感器測量和稍微不準確的運動預測，以獲得比僅來自傳感器讀數或僅有關運動的任何更好估計位置。
狀態
當定位一輛汽車時，汽車位置和運動通常被稱爲汽車狀態。 汽車的狀態包括：汽車當前位置X以及速度
X= 0 velocity = 50 initial_state = [x, velocity]
假如汽車行駛在一條單行道上： 汽車當前位置在這條路起點，以每秒50米的速度前進，因此三秒後，它將達到150米標記處
運動模型
以上是一個合理的預測，有以下假設：
- 汽車有一個初始狀態
- 假設汽車以一個恆定速度前進
distance = velocity * time該等式也稱之爲運動模型
一個更復雜的模型： 汽車依舊從零點出發，此時的速度爲50米每秒，但它以-20m/s2的加速度速度遞減
爲了解決這個問題：在狀態中包含一個新值：加速度
- distance = velocitydt + 0.5acceleration*dt2
- velocity = acceleration * dt
狀態包括此模型中的加速度，如下所示：[x, velocity, acc]。
# Constant acceleration, changing velocity # initial variables x = 0 velocity = 50 acc = -20 initial_state = [x, velocity, acc] # predicted state after three seconds have elapsed # this state has a new value for x, and a new value for velocity (but the acceleration stays the same) dt = 3 new_x = x + velocity*dt + 0.5*acc*dt**2 new_vel = velocity + acc*dt predicted_state = [new_x, new_vel, acc] # predicted_state = [60, -10, -20]
#---- 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
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)汽車對象
創建汽車對象 狀態由位置定義：[y,x];速度具有垂直分量和水平分量：[vy,vx]
import matplotlib.pyplot as plt
""" The Car class defines a car's movement and keeps track of its state.
The class includes init, move, and display functions.
This class assumes a constant velocity motion model and the state
of the car includes the car's position, and it's velocity.
Attributes:
state: A list of the car's current position [y, x] and velocity [vy, vx]
world: The world that the car is moving within (a 2D list)
"""
class Car(object):
# Car constructor
# Called when you write car.Car(_, _, _)
def __init__(self, position, velocity, world):
"""Initializes Car with some position, velocity, and a world to traverse."""
# Initialize the state
# Position is a list [y, x] and so is velocity [vy, vx]
self.state = [position, velocity]
self.world = world # world is a 2D list of values that range from 0-1
# Set the default color
self.color = 'r'
# Initalize the path
self.path = []
self.path.append(position)
# Move function
def move(self, dt=1):
""" The move function moves the car in the direction of the velocity and
updates the state.
It assumes a circular world and a default dt = 1 (though dt can be any
non-negative integer).
"""
height = len(self.world)
width = len(self.world[0])
position = self.state[0]
velocity = self.state[1]
# Predict the new position [y, x] based on velocity [vx, vy] and time, dt
predicted_position = [
(position[0] + velocity[0]*dt) % height, # default dt = 1
(position[1] + velocity[1]*dt) % width
]
# Update the state
self.state = [predicted_position, velocity]
# Every time the robot moves, add the new position to the path
self.path.append(predicted_position)
# Turn left function
def turn_left(self):
""" Turning left "rotates" the velocity values, so vy = -vx, and vx = vy.
For example, if a car is going right at 1 world cell/sec this means
vy = 0, vx = 1,
and if it turns left, then it should be moving upwards on the world grid
at the same speed!
And up is vy = -1 and vx = 0
"""
# Change the velocity
velocity = self.state[1]
predicted_velocity = [
-velocity[1],
velocity[0]
]
# Update the state velocity
self.state[1] = predicted_velocity
## TODO: Write the turn_right function
## Hint: Use turn_left for inspiration!
def turn_right(self):
""" Turning left "rotates" the velocity values, so vy = -vx, and vx = vy.
For example, if a car is going right at 1 world cell/sec this means
vy = 0, vx = 1,
and if it turns left, then it should be moving upwards on the world grid
at the same speed!
And up is vy = -1 and vx = 0
"""
# Change the velocity
velocity = self.state[1]
predicted_velocity = [
velocity[1],
-velocity[0]
]
# Update the state velocity
self.state[1] = predicted_velocity
# Helper function for displaying the world + robot position
# Assumes the world in a 2D numpy array and position is in the form [y, x]
# path is a list of positions, and it's an optional argument
def display_world(self):
# Store the current position of the car
position = self.state[0]
# Plot grid of values + initial ticks
plt.matshow(self.world, cmap='gray')
# Set minor axes in between the labels
ax=plt.gca()
rows = len(self.world)
cols = len(self.world[0])
ax.set_xticks([x-0.5 for x in range(1,cols)],minor=True )
ax.set_yticks([y-0.5 for y in range(1,rows)],minor=True)
# Plot grid on minor axes in gray (width = 2)
plt.grid(which='minor',ls='-',lw=2, color='gray')
# Create a 'x' character that represents the car
# ha = horizontal alignment, va = verical
ax.text(position[1], position[0], 'x', ha='center', va='center', color=self.color, fontsize=30)
# Draw path if it exists
if(len(self.path) > 1):
# loop through all path indices and draw a dot (unless it's at the car's location)
for pos in self.path:
if(pos != position):
ax.text(pos[1], pos[0], '.', ha='center', va='baseline', color=self.color, fontsize=30)
# Display final result
plt.show()導入和定義初始變量
# Import statements import numpy import car # Declare initial variables # Create a 2D world of 0's height = 4 width = 6 world = np.zeros((height, width)) # Define the initial car state initial_position = [0, 0] # [y, x] (top-left corner) velocity = [0, 1] # [vy, vx] (moving to the right)
創建和可視化車
# Create a car object with these initial params carla = car.Car(initial_position, velocity, world) # Display the world carla.display_world()
## TODO: Move carla around, using your new turn_right() function
## Display the result and the state as it changes
carla = car.Car(initial_position, velocity, world)
for i in range(4):
for j in range(3):
carla.move()
carla.turn_right()
carla.display_world()
重載顏色加法
class Color(object):
# Initializes a color with rgb values
def __init__(self, r, g, b):
self.r = r
self.g = g
self.b = b
# Called when a Color object is printed out
def __repr__(self):
'''Display a color swatch and returns a text description of r,g,b values'''
plt.imshow([[(self.r/255, self.g/255, self.b/255)]])
return 'r, g, b = ' + str(self.r) + ', ' + str(self.g) + ', ' + str(self.b)
## TODO: Complete this add function to add two colors together
def __add__(self, other):
'''Adds the r, g, and b components of each color together
and averaging them.
The new Color object, with these averaged rgb values,
is returned.'''
self.r = self.r + other.r
self.g = self.g + other.g
self.b = self.b + other.b
return self# Notice we are importing the color class! import numpy as np import color %matplotlib inline # Auto-reload function so that this notebook keeps up with # changes in the class file %load_ext autoreload %autoreload 2
color1 = color.Color(250, 0, 0) print(color1)
color2 = color.Color(0,0, 250) print(color2)
# Add the two colors to create a *new* color object new_color = color1 + color2 print(new_color)
狀態轉爲爲矩陣
狀態向量是一列值，比如我們感興趣位置X和速度V:
使用狀態向量，我們可以在一個矩陣乘法步驟中預測新狀態。 矩陣乘法
左邊的2*2矩陣通常稱爲狀態變換矩陣 如果x和y不相互依賴，存在單獨且恆定的x速度和y速度分量
