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

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

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

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)