专栏首页书山有路勤为径目标跟踪与定位——状态与定位

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

定位步骤

卡尔曼滤波器

回顾一下卡尔曼用语定位的步骤:

  1. 初步预测

下面是一个例子,我们知道我们的车在这条单车道上,但我们不知道它的确切位置。

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

测量更新步骤

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

预测步骤

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

卡尔曼滤波器可以结合不准确的传感器测量和稍微不准确的运动预测,以获得比仅来自传感器读数或仅有关运动的任何更好估计位置。

状态

当定位一辆汽车时,汽车位置和运动通常被称为汽车状态。 汽车的状态包括:汽车当前位置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速度分量

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

我来说两句

0 条评论
登录 后参与评论

相关文章

  • 对话框

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

    小飞侠xp
  • 事件与信号

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

    小飞侠xp
  • PyQt中布局管理

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

    小飞侠xp
  • 一日一技:如何统计Python函数被调用了几次?

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

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

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

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

    亚乐记
  • python爬虫学习,爬取豆瓣各分类书单

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

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

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

    用户1225216
  • 实战项目:飞机大战

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

    佛系编程人
  • 小蛇学python(2)两百行代码实现旅游中国34座大城市最短路径

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

    用户2145057

扫码关注云+社区

领取腾讯云代金券