# 目标跟踪与定位——状态与定位

### 定位步骤

##### 卡尔曼滤波器

1. 初步预测

1. 测量更新 感知周围的世界称之为测量更新。收集有关汽车周围环境的更多信息并改进我们的预测。 比如：测量的停车标志前两个网格单元，测量并不完美，但我们对汽车的位置有了更好的了解。

1. 预测(时间更新) 下一步是移动，也称之为时间更新或预测步骤；我们根据对速度和当前位置的了解，预测汽车的移动位置。 比如：将概率分布向右移动一个单元格

1. 循环重复 已经形成了对汽车位置的新估计，卡尔曼滤波器简单的重复感应和移动(测量和预测)，以便在汽车移动时对汽车进行定位！

##### 状态

```X= 0
velocity = 50
initial_state = [x, velocity]```

##### 运动模型

• 汽车有一个初始状态
• 假设汽车以一个恒定速度前进 `distance = velocity * time`该等式也称之为运动模型

• distance = velocitydt + 0.5acceleration*dt2
• velocity = acceleration * dt

```# 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
new_x = x+state*dt

# Create and return the new, predicted state
predicted_state = [new_x, state]
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)```
##### 汽车对象

```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
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)

position = self.state
velocity = self.state

# Predict the new position [y, x] based on velocity [vx, vy] and time, dt
predicted_position = [
(position + velocity*dt) % height, # default dt = 1
(position + velocity*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

predicted_velocity = [
-velocity,
velocity
]

# Update the state velocity
self.state = 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

predicted_velocity = [
velocity,
-velocity
]

# Update the state velocity
self.state = 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

# 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)

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, position, '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, pos, '.', 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)

'''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
```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)```

0 条评论

• ### 对话框

对话框是一个现代GUI应用不可或缺的一部分。对话是两个人之间的交流，对话框就是人与电脑之间的对话。对话框用来输入数据，修改数据，修改应用设置等等。

• ### 事件与信号

所有的应用都是事件驱动的。事件大部分都是由用户的行为产生的，当然也有其他的事件产生方式，比如网络的连接，窗口管理器或者定时器等。调用应用的exec_()方法时，...

• ### PyQt中布局管理

布局管理是GUI编程中的一个重要方面。布局管理是一种如何在应用窗口上放置组件的一种方法。我们可以通过两种基础方式来管理布局。我们可以使用绝对定位和布局类。使用布...

• ### Python3可视化爬取教务系统实现查询功能

今天来补充之前的界面操作系统，为学生查询操作系统2.0版本，依旧是用wxpython来做一个可视化的操作界面，用的工具依然是selenium库，beautifu...

• ### (二十八) 初遇python OOP面向对象编程-魔法方法(双下划线方法)

各位读者大大们大家好，今天学习python的面向对象编程-魔法方法(双下划线方法)，并记录学习过程欢迎大家一起交流分享。

• ### python爬虫学习，爬取豆瓣各分类书单

初学python会比较困难，但是只要坚定自己的信念，不轻易认输，敢于面对，成功迟早会笑脸相迎。

• ### python codis集群客户端(二) - 基于zookeeper对实例创建与摘除

在这一篇中我们实现了不通过zk来编写codis集群proxys的api， 如果codis集群暴露zk给你的话，那么就方便了，探活和故障摘除与恢复codis集群...

• ### 实战项目：飞机大战

运行程序，英雄飞机出现在屏幕底边的中央区域，飞机中央每隔一秒会自动发射一枚子弹。随着游戏的开始，敌机会在屏幕上方随机不定时的出现在屏幕内向下飞行。英雄飞机需要通...

• ### 小蛇学python（2）两百行代码实现旅游中国34座大城市最短路径

直接说基础语法，也许大家不会感兴趣。前言之后的这一章，给大家介绍一下我最近写出来的一个小功能。用python语言实现GA算法来解决TSP问题，希望以此来激发大家...

### 活动推荐 