diff --git a/student_particle_filter.py b/student_particle_filter.py
index 6e27773..71b58eb 100644
--- a/student_particle_filter.py
+++ b/student_particle_filter.py
@@ -1,106 +1,490 @@
-import numpy as np
+#!/usr/bin/env python
+"""""
+global_position_estimator_distance
+
+Implements Monte-Carlo Localization for the PiDrone
+"""""
+
 import math
-import copy
+import numpy as np
+import cv2
 
-NUM_PARTICLES = 100
-UPDATE_VARIANCE = 0.1
-TARGET = <choose a location with both x,y within 0-10>
 
+# ---------- map parameters ----------- #
+MAP_PIXEL_WIDTH = 3227  # in pixel
+MAP_PIXEL_HEIGHT = 2447
+MAP_REAL_WIDTH = 1.4  # in meter
+MAP_REAL_HEIGHT = 1.07
+# -------------------------- #
 
-class Particle:
-    """
-    Class to hold a particle in your filter. Should store its position (x,y)
-    and its weight. Note that the weight field must
-    be called "weight." Once optimized in problem 6, it should store the set of
-    all weights and poses as numpy arrays. Depending on how you implement the
-    optimization, it may also need fields or methods for retrieving the particle's
-    own weight and pose.
-    """
-    def __init__(self, x, y):
+# ----- camera parameters DO NOT EDIT ----- #
+CAMERA_WIDTH = 320
+CAMERA_HEIGHT = 240
+CAMERA_CENTER = np.float32([(CAMERA_WIDTH - 1) / 2., (CAMERA_HEIGHT - 1) / 2.]).reshape(-1, 1, 2)
+CAMERA_SCALE = 290.
+METER_TO_PIXEL = (float(MAP_PIXEL_WIDTH) / MAP_REAL_WIDTH + float(MAP_PIXEL_HEIGHT) / MAP_REAL_HEIGHT) / 2.
+# ----------------------------- #
+
+# ----- keyframe parameters ----- #
+KEYFRAME_DIST_THRESHOLD = CAMERA_HEIGHT - 40
+KEYFRAME_YAW_THRESHOLD = 0.175 # 10 degrees
+# -------------------------- #
+
+# ----- feature parameters DO NOT EDIT ----- #
+ORB_GRID_SIZE_X = 4
+ORB_GRID_SIZE_Y = 3
+MATCH_RATIO = 0.7
+MIN_MATCH_COUNT = 10
+MAP_GRID_SIZE_X = ORB_GRID_SIZE_X * 3
+MAP_GRID_SIZE_Y = ORB_GRID_SIZE_Y * 3
+CELL_X = float(MAP_PIXEL_WIDTH) / MAP_GRID_SIZE_X
+CELL_Y = float(MAP_PIXEL_HEIGHT) / MAP_GRID_SIZE_Y
+PROB_THRESHOLD = 0.001
+ORB_FEATURES_PER_GRID_CELL = 500
+# -------------------------- #
+
+
+class Particle(object):
+    """"
+    each particle holds poses and weights of all particles
+    z is currently not used
+    """""
+
+    def __init__(self, i, poses, weights):
+        self.i = i
+        self.poses = poses
+        self.weights = weights
+    #return the weight 
+    def weight(self): return self.weights[self.i]
+    #return the pose x,y,z  and yaw with 4 functions
+    def x(self): return #pose x 
+
+    def y(self): return #pose y
 
+    def z(self): return #pose z
 
-class ParticleSet:
+    def yaw(self): return #pose yaw
+
+    def __str__(self):
+        return str(self.x()) + ' , ' + str(self.y()) + ' weight ' + str(self.weight())
+
+    def __repr__(self):
+        return str(self.x()) + ' , ' + str(self.y()) + ' weight ' + str(self.weight())
+
+
+class ParticleSet(object):
+    
+    #make a class called particle set that takes an object and sets the 
+    # weight, particle list, poses and num_particles
+
+    def __init__(self, num_particles, poses):
+        self.weights = #todo
+        self.particles = #todo
+        self.poses = #todo
+        self.num_particles = #todo 
+
+
+class LocalizationParticleFilter:
     """
-    Class to hold the set of particles and the methods which will operate on them.
-    You may want to also store the target pose here as well. Remember, the goal
-    is for the (x,y) positions of the set of particles to converge on the
-    target pose. You must implement a motion model, measurement_model, and
-    resampling method.
+    Particle filter for localization.
     """
 
-    def __init__(self, num_particles):
+    def __init__(self, map_kp, map_des):
+        #todo 
+        self.map_kp = 
+        self.map_des = 
+
+        self.particles = 
+        self.measure_count = 
+
+        index_params = 
+        search_params = 
+        self.matcher = 
+        self.previous_time = 
+
+        self.key_kp = 
+        self.key_des = 
+
+        self.z = 
+        self.angle_x = 
+        self.angle_y = 
+
+        self.sigma_x = 
+        self.sigma_y = 
+        self.sigma_yaw = 
+
+        sigma_vx = 
+        sigma_vy = 
+        sigma_vz = 
+        sigma_yaw = 
+        self.covariance_motion = np.array([[sigma_vx ** 2, 0, 0, 0],
+                                           [0, sigma_vy ** 2, 0, 0],
+                                           [0, 0, sigma_vz ** 2, 0],
+                                           [0, 0, 0, sigma_yaw ** 2]])
+
+    def update(self, z, angle_x, angle_y, prev_kp, prev_des, kp, des):
         """
-        Takes the number of particles as input and creates a set of particles at
-        random positions. Note: the list of particles must be called "particles"
-        and the particle poses should be within the range 0<x<10 and 10<y<10
+        We implement the MCL algorithm from probabilistic robotics (Table 8.2)
+        kp is the position of detected features
+        des is the description of detected features
         """
+        # update parameters
+        self.z = 
+        self.angle_x = 
+        self.angle_y = 
+
+        transform = self.compute_transform(prev_kp, prev_des, kp, des)
+
+        if transform is not None:
+
+            #todo 
+            x = 
+            y =
+            # CCW rotation makes yaw more positive
+            yaw = #todo 
+
+            self.sample_motion_model(x, y, yaw)
+
+            # if there is some previous keyframe
+            if self.key_kp is not None and self.key_des is not None:
+
+                transform = #todo 
+
+                if transform is not None:
+                    # distance since previous keyframe in PIXELS
+                    x = #todo 
+                    y = #todo 
+                    yaw = #todo 
+
+                    # if we've moved an entire camera frame distance since the last keyframe (or yawed 10 degrees)
+                    if #todo 
 
-    def particle_motion(self):
+                        #call the meaurement model and define key_kp and key_des
+                        
+                else:
+                    # moved too far to transform from last keyframe, so set a new one
+                   
+
+            # there is no previous keyframe
+            else:
+               
+
+        self.resample_particles()
+        return self.get_estimated_position()
+
+    def sample_motion_model(self, x, y, yaw):
         """
-        Adds some random Gaussian noise to the x and y positions of each
-        particle in the filter. Be sure that the noise is different for each
-        particle. Use UPDATE_VARIANCE for the variance when you sample from the
-        Gaussian
-        Hint: try numpy.random.normal
+        Implement motion model from Equation 3 in PiDrone Slam with noise.
         """
+        # add noise 
+        noisy_x_y_z_yaw = np.random.multivariate_normal() #todo 
 
-    def weight_particles(self):
+        for i in range(self.particles.num_particles):
+            pose = 
+            old_yaw = pose[3]
+            #q1
+            pose[0] +=
+            pose[1] += 
+            pose[2] = 
+            pose[3] += 
+            pose[3] = 
+
+    def measurement_model(self, kp, des):
         """
-        Sets the weight of each particle inversely proportional to the
-        particle’s distance from the desired pose. ie, if a particle is close
-        to the desired pose, it should have a high weight, and if a particle
-        is far from the desired pose, it should have a low weight. Recall that
-        X is inversely proportional to Y if X = 1/Y. This weighting scheme will
-        ensure that particles closest to the target have higher weights, and
-        thus are resampled with a higher probability.
+        landmark_model_known_correspondence from probablistic robotics 6.6
         """
+        for i in range(self.particles.num_particles):
+            position = self.particles.poses[i]
+
+            # get grid x and y for position 0
+            
+
+            # get grid x and y for position 1
+           
+
+            # get 8 map cells around pose
+            sub_map_kp = self.map_kp[grid_x][grid_y]
+            sub_map_des = self.map_des[grid_x][grid_y]
+
+            # measure current global position
+            pose, num = 
+
+            # compute weight of particle
+            if pose is None:
+                q = #todo
+            else:
+                # add noise
+                noisy_pose = 
+
+                noisy_pose[3] = 
+
+                yaw_difference = 
+                #adjust the angle of the yaw difference
+                yaw_difference = 
+
+                # norm_pdf(x, 0, sigma) gets you the probability of x
+                q = 
+
+            # keep floats from overflowing
+            self.particles.weights[i] = max(q, PROB_THRESHOLD)
 
     def resample_particles(self):
+        """""
+        samples a new particle set, biased towards particles with higher weights
+        """""
+        weights_sum = np.sum(self.particles.weights)
+        new_poses = []
+        new_weights = []
+        #todo 
+        normal_weights =  # normalize
+        # samples sums to num_particles with same length as normal_weights, positions with higher weights are
+        # more likely to be sampled
+        #todo 
+        samples = 
+        for i, count in enumerate(samples):
+            for _ in range(count):
+                #get new poses and weights 
+                
+        #use the new poses and weights
+        self.particles.poses = 
+        self.particles.weights = 
+
+
+    def get_estimated_position(self):
+        """""
+        retrieves the drone's estimated position
+        """""
+        weights_sum = np.sum(self.particles.weights)
+        x = 0.0
+        y = 0
+        z = 0
+        yaw = 0
+
+        normal_weights = self.particles.weights / float(weights_sum)
+        for i, prob in enumerate(normal_weights):
+            #multiply prob with each pose 
+            x += 
+            y += 
+            z += 
+            yaw += 
+        #return the particle with all poses x,y,z,yaw
+        #also return a np.array with the sum of weights divided by the particles weights size
+        return #particle() , np.array())
+
+    def initialize_particles(self, num_particles, kp, des):
         """
-        Uses the particle weights to resample the particles and create a new set.
-        The goal is for each particle to be resampled with a probability that
-        corresponds to its weight, therefore the particle set converges on the
-        desired pose (particles closest to the target pose are more likely to be
-        resampled). This method is tricky and has been implemented for you.
+        find most possible slocation to start
+        :param num_particles: number of particles we are using
+        :param kp: the keyPoints of the first captured image
+        :param des: the descriptions of the first captured image
         """
+        self.key_kp, self.key_des = None, None
+        weights_sum = 0.0
+        weights = []
+        poses = []
+        new_poses = []
+
+        # go through every grid, trying to find matched features
+        for x in range(MAP_GRID_SIZE_X):
+            for y in range(MAP_GRID_SIZE_Y):
+                #todo
+                p, w = self.compute_location()
+                if p is not None:
+                    #append all x,y,z and yaw poses
+                    poses.append()
+                    #append weight with w
+                
+                    # add w to weights_sum
+                    
+
+        # cannot find a match
+        if len(poses) == 0:
+            print "Random Initialization"
+            for x in range(MAP_GRID_SIZE_X):
+                for y in range(MAP_GRID_SIZE_Y):
+                    #use pose.append 
+
+                    #in pose.append you will need  to add x,y,z  and a random sample
 
-        new_particles = []
+                    #divide cell x by 2.0 and add to the outcome of x * cell x
+                    # divide the outcome of the sum by _meter to pixel
+                    
+                    #divide cell y by 2.0 and add to the outcome of y * cell y
+                    # divide the outcome of the sum by _meter to pixel
 
-        # get the list of particle weights, and normalize them
-        weights = np.array([p.weight for p in self.particles])
-        weights_sum = np.sum(weights)
-        normal_weights = weights / float(weights_sum)
+                    #use z 
 
-        # choose samples from old particle set based on weights
-        samples = np.random.multinomial(self.num_particles, normal_weights)
+                    #using a random sample 
+
+
+                    poses.append(#todo)
+
+
+                    weights_sum += 1.0  # uniform sample
+                    weights.append(1.0)
+
+        # sample particles based on the number of matched features
+        weights =  # normalize
+        samples =   # sample
         for i, count in enumerate(samples):
             for _ in range(count):
-                p = copy.deepcopy(self.particles[i])
-                p.weight = PROB_THRESHOLD
-                new_particles.append(p)
+                #append new poses by i
+
+        self.particles = #particle set with new poses )
+        return self.get_estimated_position()
+
+    def compute_location(self, kp1, des1, kp2, des2):
+        """
+        compute the global location of center of current image
+        :param kp1: captured keyPoints
+        :param des1: captured descriptions
+        :param kp2: map keyPoints
+        :param des2: map descriptions
+        :return: global pose
+        """
+
+        good = []
+        pose = None
+
+        if des1 is not None and des2 is not None:
+            matches = #use a knn matcher for des1 and des2
+
+            for match in matches:
+            #if the length of match 0 is less than 1 and 
+            # the distance of match 0 is less that the ratio of match mutiplied by the distance of match 1 
+
+                if :
+                     #append good with the match at 0
+
+            #if the length of good is more than the minimal math count
+            if len(good) > MIN_MATCH_COUNT:
+    
+                # Traverse through good in terms of M  
+                # queryIdx refers to keypoints1 and trainIdx refers to keypoints2,
+                #reshape both pts with -1,1,2
+
+                src_pts = 
+                dst_pts = 
+                #estimate rigit transfrom with the src pts and dst pts
+                
+                transform = cv2.estimateRigidTransform( , , False)
+                
+                if transform is not None:
+                    transformed_center =  # get global pixel
+                    transformed_center =   # map to global pose
+                    
+                    yaw =  # get global heading
+
+                    # correct the pose if the drone is not level
+                    z = 
+                    offset_x = 
+                    offset_y = 
+                    global_offset_x = #cos
+                    global_offset_y = #sin
+                    pose = 
+
+        return pose, len(good)
+
+    def compute_transform(self, kp1, des1, kp2, des2):
+        transform = None
+
+        if des1 is not None and des2 is not None:
+            matches = #use knn match on the des1 and 2 with the k of 2
+
+            good = []
+            for match in matches:
+                #if the length of match is less than 1 and the distance of match 0 is less than the product of match ratio and match 1 distance
+                if 
+                    
+                    #append match 0 to good  
+
+
+            # queryIdx refers to keypoints1 and trainIdx refers to keypoints2,
+            #reshape both pts with -1,1,2
+                    
+            src_pts =  
+            dst_pts = 
+
+            # estimateRigidTransform needs at least three pairs
+            if src_pts is not None and dst_pts is not None and len(src_pts) > 3 and len(dst_pts) > 3:
+                #use estimate rigid transform on sec and dst pts
+                transform = 
+
+        return transform
+
+    def pixel_to_meter(self, px):
+        """""
+        uses the camera scale to convert pixel measurements into meter
+        """""
+        return px * self.z / CAMERA_SCALE
+
+
+def adjust_angle(angle):
+    """""
+    keeps angle within -pi to pi
+    """""
+    while angle > math.pi:
+        angle -= 2 * math.pi
+    while angle <= -math.pi:
+        angle += 2 * math.pi
+
+    return angle
+
+
+def create_map(file_name):
+    """
+    create a feature map, extract features from each bigger cell.
+    :param file_name: the image of map
+    :return: a list of center of bigger cells (kp and des), each bigger cell is a 3 by 3 grid (9 cells).
+    """
+
+    # read image and extract features
+    image = 
 
-        # replace the set of particles
-        self.particles = new_particles
+    max_total_keypoints = 
+    # the edgeThreshold and patchSize can be tuned if the gap between cell is too large
+    detector = 
+    kp = 
+    kp, des = 
+    # rearrange kp and des into grid
+    grid_kp = 
+    grid_des = 
+    for i in range(len(kp)):
+        x = 
+        y = 
 
+        grid_kp[x][y].append(kp[i])
+        grid_des[x][y].append(des[i])
 
+    # group every 3 by 3 grid, so we can access each center of grid and its 8 neighbors easily
+    map_grid_kp = [[[] for _ in range(MAP_GRID_SIZE_Y)] for _ in range(MAP_GRID_SIZE_X)]
+    map_grid_des = [[[] for _ in range(MAP_GRID_SIZE_Y)] for _ in range(MAP_GRID_SIZE_X)]
+    for i in range(MAP_GRID_SIZE_X):
+        for j in range(MAP_GRID_SIZE_Y):
+            for k in range(-1, 2):
+                for l in range(-1, 2):
+                    x = i + k
+                    y = j + l
+                    if 0 <= x < MAP_GRID_SIZE_X and 0 <= y < MAP_GRID_SIZE_Y:
+                        #fill in the correct values for the 2d array
+                        map_grid_kp[i][j].extend(grid_kp[][])
+                        map_grid_des[i][j].extend(grid_des[][])
 
-###############################################################################
-# Do not edit below this line
-###############################################################################
+            
+            map_grid_des[i][j] = np.array(map_grid_des[][])
 
-particles = ParticleSet(NUM_PARTICLES)
+    return map_grid_kp, map_grid_des
 
-pose_path = 'particle_filter_data.txt'
 
-try:
-    pp = open(pose_path, 'w')
-    for _ in range(500):
-        particles.particle_motion()
-        particles.weight_particles()
-        particles.resample_particles()
+def norm_pdf(x, mu, sigma):
+    u = (x - mu) / float(abs(sigma))
+    y = (1 / (np.sqrt(2 * np.pi) * abs(sigma))) * np.exp(-u * u / 2.)
+    return y
 
-        for p in particles.particles:
-            pp.write(str(p.pose[0]) + '\n' + str(p.pose[1]) + '\n')
-finally:
-    pp.close()
 
+def distance(x1, y1, x2, y2):
+    """""
+    returns the distance between two points (x1,y1) and (x2, y2)
+    """""
+    #todo
+    return math.sqrt(math.pow() + math.pow())
diff --git a/student_run_localization.py b/student_run_localization.py
index f9985fb..5a9688f 100755
--- a/student_run_localization.py
+++ b/student_run_localization.py
@@ -1,60 +1,58 @@
-#!/usr/bin/env python
-"""
-vision_localization_onboard.py
+"""""
+picam_localization_distance
 
-This file can run SLAM or localization on board, offline or online
-"""
+implements Monte-Carlo Localization using the pi-camera
+"""""
 
-
-from student_localization_helper import *
-from cv_bridge import CvBridge, CvBridgeError
-import sys
-import os
-import picamera
-import camera_info_manager
-import rospy
-from geometry_msgs.msg import PoseStamped
+import numpy as np
+import cv2
+from pidrone_pkg.msg import Mode
 from sensor_msgs.msg import Image, Range, CameraInfo
-from analyze_flow import AnalyzeFlow
 from std_msgs.msg import Empty
-from pidrone_pkg.msg import State
-import argparse
+import rospy
 import tf
+from cv_bridge import CvBridge, CvBridgeError
+from geometry_msgs.msg import PoseStamped
+from pidrone_pkg.msg import State
+from localization_helper import LocalizationParticleFilter, create_map, PROB_THRESHOLD
+import os
 
 # ---------- map parameters ----------- #
 MAP_PIXEL_WIDTH = 3227  # in pixel
 MAP_PIXEL_HEIGHT = 2447
 MAP_REAL_WIDTH = 1.4  # in meter
 MAP_REAL_HEIGHT = 1.07
+# ------------------------------------- #
 
 # ---------- camera parameters ----------- #
 CAMERA_WIDTH = 320
 CAMERA_HEIGHT = 240
-
-# assume a pixel in x and y has the same length
 METER_TO_PIXEL = (float(MAP_PIXEL_WIDTH) / MAP_REAL_WIDTH + float(MAP_PIXEL_HEIGHT) / MAP_REAL_HEIGHT) / 2.
 CAMERA_CENTER = np.float32([(CAMERA_WIDTH - 1) / 2., (CAMERA_HEIGHT - 1) / 2.]).reshape(-1, 1, 2)
+# ---------------------------------------- #
 
 # ---------- localization parameters ----------- #
 MAX_BAD_COUNT = -10
 NUM_PARTICLE = 30
 NUM_FEATURES = 200
+# ---------------------------------------------- #
 
 
-class Localizer(picamera.array.PiMotionAnalysis):
+class AnalyzePhase:
 
-    def __init__(self, camera, bridge):
-        picamera.array.PiMotionAnalysis.__init__(self, camera)
-        self.bridge = bridge
+    def __init__(self):
+        self.bridge = CvBridge()
         self.br = tf.TransformBroadcaster()
 
-        self.posepub = rospy.Publisher('/pidrone/picamera/pose', PoseStamped, queue_size=1)
-        self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image", Image, queue_size=1, latch=True)
-
-        self.detector = cv2.ORB_create(nfeatures=NUM_FEATURES, scoreType=cv2.ORB_FAST_SCORE)
+        self.posepub = 
+        self.first_image_pub = 
+        #opencv orb  create
+        self.detector = 
+        #use map.jpg to create a map
+        map_grid_kp, map_grid_des = 
 
-        map_grid_kp, map_grid_des = create_map('map.jpg')
-        self.estimator = LocalizationParticleFilter(map_grid_kp, map_grid_des)
+        #use map grid kp and des inside the localizationparticlefilter
+        self.estimator = 
 
         # [x, y, z, yaw]
         self.pos = [0, 0, 0, 0]
@@ -81,65 +79,70 @@ def __init__(self, camera, bridge):
         self.alpha_yaw = 0.1  # perceived yaw smoothing alpha
         self.hybrid_alpha = 0.3  # blend position with first frame and int
 
-    def write(self, data):
-        curr_img = np.reshape(np.fromstring(data, dtype=np.uint8), (CAMERA_HEIGHT, CAMERA_WIDTH, 3))
-        curr_rostime = rospy.Time.now()
-        self.posemsg.header.stamp = curr_rostime
-        curr_time = curr_rostime.to_sec()
+    #todo image callback
+    def image_callback(self, data):
+       
+        curr_img = 
+        curr_rostime = =
+        self.posemsg.header.stamp = 
+        curr_time = 
 
+        #todo
         # start MCL localization
         if self.locate_position:
-            curr_kp, curr_des = self.detector.detectAndCompute(curr_img, None)
+            curr_kp, curr_des = 
 
             if curr_kp is not None and curr_kp is not None:
                 # generate particles for the first time
                 if self.first_locate:
-                    particle = self.estimator.initialize_particles(NUM_PARTICLE, curr_kp, curr_des)
-                    self.first_locate = False
-                    self.pos = [particle.x(), particle.y(), particle.z(), particle.yaw()]
+                    particle = 
+                    self.first_locate = 
+                    self.pos = 
 
-                    self.posemsg.pose.position.x = particle.x()
-                    self.posemsg.pose.position.y = particle.y()
-                    self.posemsg.pose.position.z = particle.z()
-                    x, y, z, w = tf.transformations.quaternion_from_euler(0, 0, self.pos[3])
+                    self.posemsg.pose.position.x = 
+                    self.posemsg.pose.position.y = 
+                    self.posemsg.pose.position.z = 
+                    x, y, z, w = 
 
-                    self.posemsg.pose.orientation.x = x
-                    self.posemsg.pose.orientation.y = y
-                    self.posemsg.pose.orientation.z = z
-                    self.posemsg.pose.orientation.w = w
+                    self.posemsg.pose.orientation.x = 
+                    self.posemsg.pose.orientation.y = 
+                    self.posemsg.pose.orientation.z = 
+                    self.posemsg.pose.orientation.w = 
 
-                    self.posepub.publish(self.posemsg)
                     print 'first', particle
                 else:
-                    particle = self.estimator.update(self.z, self.angle_x, self.angle_y, self.prev_kp, self.prev_des,
-                                                     curr_kp, curr_des)
+                    particle = 
 
                     # update position
-                    self.pos = [self.hybrid_alpha * particle.x() + (1.0 - self.hybrid_alpha) * self.pos[0],
-                                self.hybrid_alpha * particle.y() + (1.0 - self.hybrid_alpha) * self.pos[1],
-                                self.z,
-                                self.alpha_yaw * particle.yaw() + (1.0 - self.alpha_yaw) * self.pos[3]]
+                    self.pos = 
 
-                    self.posemsg.pose.position.x = self.pos[0]
-                    self.posemsg.pose.position.y = self.pos[1]
-                    self.posemsg.pose.position.z = self.pos[2]
-                    x, y, z, w = tf.transformations.quaternion_from_euler(0, 0, self.pos[3])
 
-                    self.posemsg.pose.orientation.x = x
-                    self.posemsg.pose.orientation.y = y
-                    self.posemsg.pose.orientation.z = z
-                    self.posemsg.pose.orientation.w = w
+                    self.posemsg.pose.position.x = 
+                    self.posemsg.pose.position.y =
+                    self.posemsg.pose.position.z = 
+                    x, y, z, w = 
 
+                    self.posemsg.pose.orientation.x = 
+                    self.posemsg.pose.orientation.y = 
+                    self.posemsg.pose.orientation.z = 
+                    self.posemsg.pose.orientation.w = 
                     self.posepub.publish(self.posemsg)
+
                     print '--pose', self.pos[0], self.pos[1], self.pos[3]
 
                     # if all particles are not good estimations
+                    #todo
                     if is_almost_equal(particle.weight(), PROB_THRESHOLD):
-                        self.map_counter = self.map_counter - 1
+                        #subtract map counter by 1 
+                        self.map_counter = 
                     elif self.map_counter <= 0:
+                        #map counter equals 1 
                         self.map_counter = 1
                     else:
-                        self.map_counter = min(self.map_counter + 1, -MAX_BAD_COUNT)
+                        #use the min() function 
+                        #min()has 2 arguments
+                        #add to map counter and use -MAX_BAD_COUNT
+                        self.map_counter = 
 
                     # if it's been a while without a significant average weight
                     if self.map_counter < MAX_BAD_COUNT:
@@ -165,68 +168,41 @@ def write(self, data):
 
     def state_callback(self, data):
         """ update z, angle x, and angle y data when /pidrone/state is published to """
-        self.z = data.pose_with_covariance.pose.position.z
-        self.angle_x = data.twist_with_covariance.twist.angular.x
-        self.angle_y = data.twist_with_covariance.twist.angular.y
+       #update with converiance
+        self.z = 
+        self.angle_x = 
+        self.angle_y = 
 
     def reset_callback(self, data):
         """start localization when '/pidrone/reset_transform' is published to (press 'r')"""
         print "Start localization"
-        self.locate_position = True
-        self.first_locate = True
-        self.map_counter = 0
-        self.max_map_counter = 0
+        #SET TO TRUE
+         
+        #SET TO ZERO
+         
 
 
 def is_almost_equal(x, y):
-    epsilon = 1e-8
+    epsilon = 1*10**(-8)
     return abs(x-y) <= epsilon
 
 
 def main():
     node_name = os.path.splitext(os.path.basename(__file__))[0]
     rospy.init_node(node_name)
-
-    image_pub = rospy.Publisher("/pidrone/picamera/image_raw", Image, queue_size=1, tcp_nodelay=False)
-    camera_info_pub = rospy.Publisher("/pidrone/picamera/camera_info", CameraInfo, queue_size=1, tcp_nodelay=False)
-
-    cim = camera_info_manager.CameraInfoManager("picamera", "package://pidrone_pkg/params/picamera.yaml")
-    cim.loadCameraInfo()
-    if not cim.isCalibrated():
-        rospy.logerr("warning, could not find calibration for the camera.")
-
-    try:
-        bridge = CvBridge()
-
-        with picamera.PiCamera(framerate=90) as camera:
-            camera.resolution = (CAMERA_WIDTH, CAMERA_HEIGHT)
-            with AnalyzeFlow(camera) as flow_analyzer:
-                flow_analyzer.setup(camera.resolution)
-                phase_analyzer = Localizer(camera, bridge)
-
-                rospy.Subscriber('/pidrone/reset_transform', Empty, phase_analyzer.reset_callback)
-                rospy.Subscriber('/pidrone/state', State, phase_analyzer.state_callback)
-
-                camera.start_recording("/dev/null", format='h264', splitter_port=1, motion_output=flow_analyzer)
-                print "Starting Flow"
-                camera.start_recording(phase_analyzer, format='bgr', splitter_port=2)
-                last_time = None
-                while not rospy.is_shutdown():
-                    camera.wait_recording(1 / 100.0)
-
-                    if phase_analyzer.prev_img is not None and phase_analyzer.prev_time != last_time:
-                        image_message = bridge.cv2_to_imgmsg(phase_analyzer.prev_img, encoding="bgr8")
-                        image_message.header.stamp = phase_analyzer.prev_rostime
-                        last_time = phase_analyzer.prev_rostime
-                        image_pub.publish(image_message)
-                        camera_info_pub.publish(cim.getCameraInfo())
-
-                camera.stop_recording(splitter_port=1)
-                camera.stop_recording(splitter_port=2)
-        print "Shutdown Received"
-    except Exception:
-        print "Camera Error!!"
-        raise
+    
+    phase_analyzer = AnalyzePhase()
+s
+    phase_analyzer = AnalyzePhase()
+    #subcriber to reset_transform , raspicam_node and pidrone_state 
+    #with the correct callback
+    rospy.Subscriber("")
+    rospy.Subscriber("")
+    rospy.Subscriber("")
+
+    print"Start")
+
+    rospy.spin()
 
 
 if __name__ == '__main__':
diff --git a/student_slam_helper.py b/student_slam_helper.py
index 0141c57..f13301e 100644
--- a/student_slam_helper.py
+++ b/student_slam_helper.py
@@ -1,79 +1,130 @@
 """
-slam_helper.py
-
-Implements fastSLAM for the pidrone
+student_localization_helper
+
+Fill in the methods according to the docstrings in order to implement
+localization on your drone. Run student_run_localization to test your
+implementation. We expect that you will re-use code from the particle filter and
+OpenCV assignments to complete this task! You should not need to define
+any other methods to complete this task, though you may do so if you feel like
+it will help you.
 """
 
-import numpy as np
 import math
-import utils
-import copy
+import numpy as np
 import cv2
 
-# set this to true to write your SLAM output to a file to be animated
-WRITE_TO_FILE  = True
 
-# ----- camera parameters DO NOT EDIT ------- #
-CAMERA_SCALE = 290.
-CAMERA_WIDTH = 320.
-CAMERA_HEIGHT = 240.
-MATCH_RATIO = 0.7
+# ---------- map parameters ----------- #
+MAP_PIXEL_WIDTH = 3227  # in pixel
+MAP_PIXEL_HEIGHT = 2447
+MAP_REAL_WIDTH = 1.4  # in meter
+MAP_REAL_HEIGHT = 1.07
+# change the above in order to use a different map 
 
-# ----- SLAM parameters ------- #
-PROB_THRESHOLD = 0.005
-KEYFRAME_DIST_THRESHOLD = CAMERA_HEIGHT
-KEYFRAME_YAW_THRESHOLD = 0.175
+# ----- camera parameters DO NOT EDIT ----- #
+CAMERA_WIDTH = 320
+CAMERA_HEIGHT = 240
+CAMERA_CENTER = np.float32([(CAMERA_WIDTH - 1) / 2., (CAMERA_HEIGHT - 1) / 2.]).reshape(-1, 1, 2)
+CAMERA_SCALE = 290.
+METER_TO_PIXEL = (float(MAP_PIXEL_WIDTH) / MAP_REAL_WIDTH + float(MAP_PIXEL_HEIGHT) / MAP_REAL_HEIGHT) / 2.
 
-# ----- edit to where you want the SLAM data written --------- #
-path = 'pose_data.txt'
+# ----- map feature parameters DO NOT EDIT ----- #
+ORB_GRID_SIZE_X = 4
+ORB_GRID_SIZE_Y = 3
+MATCH_RATIO = 0.7
+MIN_MATCH_COUNT = 10
+MAP_GRID_SIZE_X = ORB_GRID_SIZE_X * 3
+MAP_GRID_SIZE_Y = ORB_GRID_SIZE_Y * 3
+CELL_X = float(MAP_PIXEL_WIDTH) / MAP_GRID_SIZE_X
+CELL_Y = float(MAP_PIXEL_HEIGHT) / MAP_GRID_SIZE_Y
+PROB_THRESHOLD = 0.001 # set the weights of new particles to this value
+MAP_FEATURES = 600
 
 
-class Particle:
+class Particle(object):
     """
-    attributes:
-    pose:           a list giving the robot's position (x, y, z, yaw)
-    landmarks:      a list of landmark objects
-    weight:         the current weight for the particle
+    Each particle holds the complete set of poses and weights for each
+    of the particles in the filter. Note that z position should be stored by
+    the particle but is not estimated by the particle filter. Each item
+    in the poses list will be the list [x,y,z,yaw].
     """
 
-    def __init__(self, x, y, z, yaw):
-        self.pose = [x, y, z, yaw]
-        self.landmarks = []
-        self.weight = PROB_THRESHOLD
+    def __init__(self, i, poses, weights):
+        self.i = i
+        self.poses = poses
+        self.weights = weights
+
+    def weight(self): return self.weights[self.i]
+
+    def x(self): return self.poses[self.i, 0]
+
+    def y(self): return self.poses[self.i, 1]
+
+    def z(self): return self.poses[self.i, 2]
+
+    def yaw(self): return self.poses[self.i, 3]
 
     def __str__(self):
-        return "Pose: " + str(self.pose) + " Weight: " + str(self.weight)
+        return str(self.x()) + ' , ' + str(self.y()) + ' weight ' + str(self.weight())
 
+    def __repr__(self):
+        return str(self.x()) + ' , ' + str(self.y()) + ' weight ' + str(self.weight())
 
-class FastSLAM:
-    def __init__(self):
-        self.particles = None
-        self.num_particles = None
-        self.weight = PROB_THRESHOLD
 
-        self.z = 0
-        self.perceptual_range = 0.0
+class ParticleSet(object):
+    """
+    Stores the set of particles.
+    """
+
+    def __init__(self, num_particles, poses):
+        """
+        Take the set of poses and the number of particles as input and creates 
+        the set of particles. You may assign to each particle the weight 
+        PROB_THRESHOLD.
+        """
+
+        #######################################################################
+        # TODO implement this class 
+        # 1 Create the list of particle weights, saved as a 'weights' field
+        # 2 Create the list self.particles where each particle gets initialized
+        #   with with its number, the list of poses, and the list of weights
+        # 3 save self.poses and self.num_particles as fields                                        
+        #######################################################################
 
-        # key points and descriptors from the most recent keyframe
-        self.key_kp = None
-        self.key_des = None
 
-        if WRITE_TO_FILE:
-            self.file = open(path, 'w')
+class LocalizationParticleFilter:
+    """
+    Particle filter for localization. Contains all of the methods and parameters
+    necessary to implement the Monte Carlo localization algorithm. You should
+    not need to edit anything in the init method below.
+    """
+
+    def __init__(self, map_kp, map_des):
+        self.map_kp = map_kp
+        self.map_des = map_des
+
+        self.particles = None
+        self.measure_count = 0
 
-        # --------------- openCV parameters --------------------- #
         index_params = dict(algorithm=6, table_number=6, key_size=12, multi_probe_level=1)
         search_params = dict(checks=50)
         self.matcher = cv2.FlannBasedMatcher(index_params, search_params)
 
-        # ------- parameters for noise on observations ----------- #
-        self.sigma_d = 3
-        self.sigma_p = .30
-        self.sigma_observation = np.array([[self.sigma_d ** 2, 0], [0, self.sigma_p ** 2]])
+        self.previous_time = None
 
-        # ------- parameters for noise on motion updates --------- #
-        sigma_vx = 2
-        sigma_vy = 2
+        self.key_kp = None
+        self.key_des = None
+
+        self.z = 0.0
+        self.angle_x = 0.0
+        self.angle_y = 0.0
+
+        self.sigma_x = 0.05
+        self.sigma_y = 0.05
+        self.sigma_yaw = 0.01
+
+        sigma_vx = 0.01
+        sigma_vy = 0.01
         sigma_vz = 0.0
         sigma_yaw = 0.01
         self.covariance_motion = np.array([[sigma_vx ** 2, 0, 0, 0],
@@ -81,338 +132,376 @@ def __init__(self):
                                            [0, 0, sigma_vz ** 2, 0],
                                            [0, 0, 0, sigma_yaw ** 2]])
 
-    def generate_particles(self, num_particles):
-        """
-        Creates the initial set of particles for SLAM
-        Each particle should be initialized at (0,0) since we build the map relative to the drone's
-        initial position, but with some noise.
-
-        :param num_particles: the number of particles to generate
+    def update(self, z, angle_x, angle_y, prev_kp, prev_des, kp, des):
         """
+        We implement the MCL algorithm from probabilistic robotics (Table 8.2)
+        :param kp: the positions of detected features
+        :param des: the descriptors of detected features
+        :param prev_kp, prev_des: kp and des from the previous frame
+        :param angle_x, angle_y: the current roll and pitch of the drone
+        :param z: the current height of the drone (not filtered)
+        :return: the drone's position estimate
+
+        Do a motion update on every frame, and a measurement update whenever
+        there is a keyframe (the drone has moved a certain distance since the
+        last time we determined a keyframe).You must choose a threshold for 
+        the distance which must pass between camera frames to trigger a measurement 
+        update.
+
+        The kp and des of the previous keyframe should be stored in self.kp
+        and self.des. You should use self.compute_transform to find the 
+        distance between the previous camera frame and the current one 
+        to use as a control input for the motion model. 
+
+        You must then determine when to perform a measurement update by 
+        comparing the current image to the last keyframe image. Do this by
+        comparing the current kp and des to self.key_kp and self.key_des. Make sure
+        you consider what will happen when:
+        1. there is no previous keyframe
+        2. the camera has moved too far to transform from the previous keyframe
         
-        ##############################################################################
-        # TODO Initialize the set of particles for FASTSLAM with x,y poses around 0,
-        #      z initialized to self.z, and yaw intialized to pi. Be sure to add noise
-        #      to the z,y, and yaw initial estimates!
-        # Be sure to call it 'self.particles'
-
-        ##############################################################################
-
-        # Reset SLAM variables in case of restart
-        self.num_particles = num_particles
-        self.key_kp, self.key_des = None, None
-        self.weight = PROB_THRESHOLD
-
-        return estimate_pose(self.particles)
-
-    def run(self, z, prev_kp, prev_des, kp, des):
-        """
-        Applies an iteration of the FastSLAM algorithm
-
-        :param z: the new infrared height estimate for the drone
-        :param prev_kp, prev_des: the keypoints and descriptors from the previous frame
-        :param kp, des: the current keypoints and descriptors
+        Also consider when it is necessary to resample!
         """
-
-        # ---------------- FILE WRITING CODE DO NOT EDIT ----------------------------- #
-        if WRITE_TO_FILE:
-            if self.particles is not None and self.particles[0].landmarks != []:
-                # write the poses of all the particles
-                self.file.write(str([[p.pose[0], p.pose[1]] for p in self.particles]) + '\n')
-                # write the landmark poses for the first particle
-                self.file.write(str([[lm.x, lm.y] for lm in self.particles[0].landmarks]) + '\n')
-                # write the GLOBAL poses of all the currently observed features relative to first particle
-                poses = []
-                for k in kp:
-                    kp_x, kp_y = k[0], k[1]
-                    # key point y is measured from the top left
-                    kp_y = CAMERA_HEIGHT - kp_y
-
-                    dx = kp_x - CAMERA_WIDTH / 2
-                    dy = kp_y - CAMERA_HEIGHT / 2
-                    pose = [self.pixel_to_meter(dx) + self.particles[0].pose[0],
-                            self.pixel_to_meter(dy) + self.particles[0].pose[1]]
-                    poses.append(pose)
-                self.file.write(str(poses) + '\n')
-        # ------------------------------------------------------------------------------ #
-
+        # update parameters
         self.z = z
+        self.angle_x = angle_x
+        self.angle_y = angle_y
 
-        # reflect that the drone can see more or less if its height changes
-        self.update_perceptual_range()
-
-        ##################################################################################
-        # TODO implement the remainder of this method
+        #######################################################################
+        # TODO: implement this method!
         #
-        # Just as in localization, you will need to update each particle with the motion
-        # estimate given by "utils.compute_transform"
-        # Then, you should call self.detect_keyframe to determine whether to perform
-        # a map update
-        ##################################################################################
-
-        return estimate_pose(self.particles), self.weight
+        # Note: if transform is the output of self.compute_transform then:
+        #
+        # x = -transform[0, 2]
+        # y = transform[1, 2]
+        # yaw = -np.arctan2(transform[1, 0], transform[0, 0])
+        # all of which are measured in pixels
+        #
+        # be sure to return the estimated position 
+        #######################################################################
 
-    def predict_particle(self, particle, x, y, yaw):
+    def motion_model(self, x, y, yaw):
         """
-        Updates this particle's position according to the transformation from prev frame to this one.
-        The robot's new position is determined based on the control, with added noise to account for control error
-
-        :param particle:  the particle containing the robot's position information
-        :param x, y, yaw: the "controls" computed by transforming the previous camera frame to this one to
-                          find the dislplacement, NOTE these are measured in pixels!
+        Update the x,y, and yaw poses of each particle in the filter using the
+        distance between the previous camera frame and this one as a motion
+        estimate. Be sure to add Gaussian noise to the x,y,yaw distance values
+        and use different noise for each particle!
+
+        Hint: use the predefined self.covariance_motion for the covariance as 
+        an input to numpy.random.multivariate_normal to sample from the normal 
+        distribution. You will likely need to look at the documentation of this 
+        function.
+
+        :param x: the estimated distance in x between the previous frame and this
+                  one, in meters
+        :param y: the estimated distance in y between the previous frame and this
+                  one, in meters
+        :param yaw: the estimated yaw difference between the previous frame and
+                    this one, in radians
+        :return: nothing
         """
-        ##############################################################################
-        # TODO use the x,y,yaw transformation between the previous frame and this one
-        #      to update the pose of particle. Be sure to add different noise for each
-        #      dimension and you may set the z pose of each particle to self.z
 
-        ##############################################################################
+        #######################################################################
+        # TODO implement this method
+        #######################################################################
 
-    def detect_keyframe(self, kp, des):
+    def measurement_model(self, kp, des):
         """
-        Performs a map update if 
-        1) there is not a previous keyframe
-        2) the distance between the previous keyframe and the current frame is above a threshold
-        3) we cannot transform between the previus frame and this one. 
-
-        :param kp, des: the lists of keypoints and descriptors
+        The measurement model sets the weight for each particle in the filter.
+        The weight should reflect the error between the pose of a particle and
+        the "true" pose of the robot. We will obtain the true pose by computing 
+        the location of the drone's current image in the larger map.
+
+        To implement this method, you should follow the "landmark model known
+        correspondence" algorithm in the textbook online.
+
+        We have provided a bit of code which will get the kp and
+        des from the 8 map grids immediately surrounding the pose of each
+        particle. Your job is to compute the probability that each particle's pose is
+        the correct pose of the drone and set the weight of the particle to that
+        probability.
+
+        We have provided a function called "norm pdf" which allows you to obtain
+        the probability of a particular sample given a mean and variance. See the
+        docstring on the method in order to use it. You should use this method
+        to determine the probability of each particle's error. Specifically, use
+        norm_pdf with a mean of zero, which will give you the probability of drawing
+        a sample from the normal distribution with a mean of zero, ie numbers far 
+        from zero have lower probability of being sampled
+        from this distribution. The sample you should consider is the error
+        between the pose of each particle and the true pose of the drone.
+
+        :param kp: the currently observed keypoints
+        :param des: the currently observed descriptors
+        :return: nothing
         """
-        ##############################################################################
-        # TODO implement this method, please call self.update_map to update the map
-        #
-        # Note that if transform is the transformation matrix from utils.compute_transform
-        #       then: x = -transform[0, 2]
-        #             y = transform[1,2]
-        #             yaw = -np.arctan2(transform[1, 0], transform[0, 0])
-        ##############################################################################
+        for i in range(self.particles.num_particles):
+            position = self.particles.poses[i]
+
+            # get grid x and y
+            grid_x = int(position[0] * METER_TO_PIXEL / CELL_X)
+            grid_x = max(min(grid_x, MAP_GRID_SIZE_X - 1), 0)
+            grid_y = int(position[1] * METER_TO_PIXEL / CELL_Y)
+            grid_y = max(min(grid_y, MAP_GRID_SIZE_Y - 1), 0)
+            # get 8 map cells around pose
+            sub_map_kp = self.map_kp[grid_x][grid_y]
+            sub_map_des = self.map_des[grid_x][grid_y]
+
+            # measure current global position
+            pose, num = self.compute_location(kp, des, sub_map_kp, sub_map_des)
+
+            #######################################################################
+            # TODO implement the pseucode below!
+            #
+            # Note: use self.sigma_x/y/yaw to use as the variance for norm_pdf and
+            # for np.random.normal
+            #
+            # 1 add some noise to the global pose (pose)
+            # 2 find the errors between the global pose (pose) and the pose of 
+            #   each particle (position)
+            # 3 call adjust_angle on the difference of yaw to keep it within
+            #   -pi/2 and pi/2
+            # 4 find the probabilities of the errors using norm_pdf (see docstring above)
+            # 5 set the weight of the particle to the max of PROB_THRESHOLD and the
+            #   product of the probabilities for the error in x, y, and yaw
+            #######################################################################
 
-    def update_map(self, kp, des):
+    def resample_particles(self):
+        """
+        Samples a new particle set, reproducing each particle with a probability
+        proportional to its weight. 
         """
-        Perform a map update on every particle, sets self.weight to the average particle weight, 
-        resamples the set of particles, and sets the new keyframe to the current frame.
 
-        Please do not edit this method
+        weights_sum = np.sum(self.particles.weights)
+        new_poses = []
+        new_weights = []
 
-        :param kp, des: the lists of keypoints and descriptors
-        """
-        # TODO implement this method according to the docstring
-        # note: we have provided self.get_average_weight
+        normal_weights = self.particles.weights / float(weights_sum)  # normalize
+        # samples sums to num_particles with same length as normal_weights, 
+        # positions with higher weights are more likely to be sampled
+        samples = np.random.multinomial(self.particles.num_particles, normal_weights)
+        for i, count in enumerate(samples):
+            for _ in range(count):
+                new_poses.append(self.particles.poses[i])
+                new_weights.append(self.particles.weights[i])
+
+        self.particles.poses = np.array(new_poses)
+        self.particles.weights = np.array(new_weights)
 
-    def update_particle(self, particle, keypoints, descriptors):
+    def get_estimated_position(self):
         """
-        :param particle: the old particle to perform the data association on
-        :param keypoints, descriptors: the lists of currently observed keypoints and descriptors
-
-        This function is the meat of the SLAM algorithm and the main difference from your 
-        localization code. Here, you will associate the currently observed features with
-        the set of landmarks in each particle. You will consider each detected feature and try to 
-        match it on the set of landmarks in the particle within a close range (self.perceptual_range) 
-        to the drone. This ensures that the map in each particles is conditioned on the robot path
-        represented by that particle alone.
-
-        If there is a decent match (see your code from the OpenCV assignment for determining match quality),
-        then you will update the ekf of that landmark with the new feature's location by calling utils.update_landmark
-        and if the match is poor, then you will add the feature as a new landmark with utils.add_landmark.
-
-        We should take adding a new landmark into account for the particle's weight since
-        expanding the map reduces certainty, so subtract from the particle's weight when you do this.
-        When updating a landmark, you should adjust the weight of the particle by some number proportional to the 
-        quality of the match to reflect how certain we are that the observed feature matches
-        the existing landmark.
-
-        Finally, we need a mechanism to ensure that dubious, or low quality, landmarks are removed from the
-        map. To do this, we will keep track of the number of times a landmark in the map is matched to by
-        newly observed features, and if it not seen for a while, we will remove that landmark.
-
-        Here, we include the function signatures for the methods from utils which you will need to 
-        implement update_particle:
-
-        def add_landmark(particle, kp, des, sigma_observation, kp_to_measurement):
-            adds a newly observed landmark to particle
-
-            :param particle: the particle to add the new landmark to
-            :param kp, des: the keypoint and descriptor of the new landmark to add
-            :param sigma_observation: the covariance of the observation measurement
-            :param kp_to_measurement: a function which computes a range/bearing measurement from the center of the
-
-        def update_landmark(particle, landmark, kp, des, sigma_observation, kp_to_measurement):
-            update the mean and covariance of a landmark
-
-            uses the Extended Kalman Filter (EKF) to update the existing landmark's mean (x, y) and
-            covariance according to the new measurement
-
-            :param particle: the particle to update
-            :param landmark: the landmark to update
-            :param kp, des: the keypoint and descriptor of the new landmark
-            :param sigma_observation: the covariance of the observation measurement
-            :param kp_to_measurement: a function which computes a range/bearing measurement from the center of the
-                                      camera frame to kp
-            :return: the updated landmark
+        Return the drone's estimated position using the expectation of the
+        belief distribution, which is the weighted sum of the poses of all the
+        particles in the filter, weighted by the weight of the particle. Note
+        that the weights must be normalized to 1 for this to work.
+
+        eg: the estimated x position of the drone is the sum over all of the
+              particles of the product of the normalized weight of the
+              particle and the particle's x pose
         """
 
-        particle.weight = PROB_THRESHOLD
+        #######################################################################
+        # TODO implement this method
 
-        ##############################################################################
-        # TODO implement this method to update the map for FastSLAM! 
-        #
-        # if this particle has no landmarks:
-        #    add every currently observed feature as a landmark
-        #    adjust particle's weight to reflect the change in certainty from adding landmarks
-        # else:
-        #    close landmarks = list of particle's landmarks within self.perceptual_range of the particle's pose
-        #    if close landmarks is not empty:
-        #        part_descriptors = list of landmark.des for each landmark in close_landmarks
-        #        matched_landmarks = boolean list with same length as close_landmarks, set to false
-        #
-        #    for each observed feature:
-        #        if part_descriptors is not empty:
-        #            match = self.matcher.knnMatch(np.array([des]), np.array(part_descriptors), k=2)
-        #
-        #        if there was not match or the match is poor quality:
-        #            add the feature as a new landmark
-        #            adjust particle weight to reflect the change in certainty
-        #        else:
-        #            use match.trainIdx to find the index of the matching landmark in the map
-        #            set the boolean corresponding to this landmark in matched_landmarks to true
-        #            update the landmark in particle.landmarks with information from the newly
-        #               observed feature*
-        #            update the weight of the particle to reflect the quality of the match
-        #               hint: use scale_weight
-        #
-        # * this step is the most tricky because the index of the landmark in the close_landmarks list
-        #   is not the same as its index in particle.landmarks! This is because close_landmarks only
-        #   contains some of the landmarks in particle.landmarks
-        ##############################################################################
-
-            # this bit of code removes dubious landmarks from the particle
-            if matched_landmarks is not None:
-                # increment counter for revisited particles, and decrement counter for non-revisited particles
-                removed = []
-                for i, match in enumerate(matched_landmarks):
-                    tup = close_landmarks[i]
-                    lm = particle.landmarks[tup[1]]
-                    if match:
-                        lm.counter += 1
-                    else:
-                        lm.counter -= 1
-                        # 'punish' this particle for having a dubious landmark
-                        particle.weight += math.log(0.1*PROB_THRESHOLD)
-                        if lm.counter < 0:
-                            removed.append(lm)
-
-                for rm in removed:
-                    particle.landmarks.remove(rm)
-
-    def get_average_weight(self):
-        """
-        the average weight of all the particles
+        # 1. compute the estimated values x, y, z, yaw 
+        # 2. compute the average weight value: avg_weight
+        #######################################################################
 
-        :param particles: the set of particles whose weight will be averaged
-        """
-        return np.sum([p.weight for p in self.particles]) / float(self.num_particles)
+        return Particle(0, np.array([[x, y, z, yaw]]), np.array([avg_weight]))
 
-    def pixel_to_meter(self, px):
+    def initialize_particles(self, num_particles, kp, des):
         """
-        uses the camera scale to convert pixel measurements into meters
-
-        :param px: the distance in pixels to convert
+        find most possible location to start
+        :param num_particles: number of particles we are using
+        :param kp: the keyPoints of the first captured image
+        :param des: the descriptions of the first captured image
         """
-        ##############################################################################
-        # TODO paste your code from localization to convert pixels to meters
-        ##############################################################################
+        self.key_kp, self.key_des = None, None
+        weights_sum = 0.0
+        weights = []
+        poses = []
+        new_poses = []
+
+        # go through every grid, trying to find matched features
+        for x in range(MAP_GRID_SIZE_X):
+            for y in range(MAP_GRID_SIZE_Y):
+                p, w = self.compute_location(kp, des, self.map_kp[x][y], self.map_des[x][y])
+                if p is not None:
+                    poses.append([p[0], p[1], p[2], p[3]])
+                    weights.append(w)
+                    weights_sum += w
+
+        # cannot find a match
+        if len(poses) == 0:
+            print "Random Initialization"
+            for x in range(MAP_GRID_SIZE_X):
+                for y in range(MAP_GRID_SIZE_Y):
+                    poses.append([(x * CELL_X + CELL_X / 2.0) / METER_TO_PIXEL,
+                                  (y * CELL_Y + CELL_Y / 2.0) / METER_TO_PIXEL,
+                                  self.z,
+                                  np.random.random_sample() * 2 * np.pi - np.pi])
+                    weights_sum += 1.0  # uniform sample
+                    weights.append(1.0)
+
+        # sample particles based on the number of matched features
+        weights = np.array(weights) / weights_sum  # normalize
+        samples = np.random.multinomial(num_particles, weights)  # sample
+        for i, count in enumerate(samples):
+            for _ in range(count):
+                new_poses.append(poses[i])
 
-    def kp_to_measurement(self, kp):
+        self.particles = ParticleSet(num_particles, np.array(new_poses))
+        return self.get_estimated_position()
+        
+    def compute_transform(self, kp1, des1, kp2, des2):
         """
-        Computes the range and bearing from the center of the camera frame to (kp.x, kp.y)
-        bearing is in (-pi/2, pi/2) measured in the standard math way
-
-        :param kp: they keypoint to measure from
+        Returns the pixel transformation matrix (output of estimateRigidTransform)
+        from kp1 to kp2. This is used to find the distance between camera frames.
+        This should be entirely code from the OpenCV assignment!
+
+        :param kp1: the first list of keypoints
+        :param des1: the first list of descriptors
+        :param kp2: the second list of keypoints
+        :param des2: the second list of descriptors
+        :return: the transformation matrix from kp1 to kp2
         """
-        kp_x, kp_y = kp[0], kp[1]
-        # key point y is measured from the top left
-        kp_y = CAMERA_HEIGHT - kp_y
 
-        dx = kp_x - CAMERA_WIDTH / 2
-        dy = kp_y - CAMERA_HEIGHT / 2
-        dist = math.sqrt(math.pow(dx, 2) + math.pow(dy, 2))
-        dist = self.pixel_to_meter(dist)
-        bearing = math.atan2(dy, dx)
+        #######################################################################
+        # TODO fill in this method with your code from the OpenCV task
+        #######################################################################
 
-        return dist, bearing
 
-    def update_perceptual_range(self):
+    def compute_location(self, kp1, des1, kp2, des2):
         """
-        computes the perceptual range of the drone: the distance from the center of the frame to the
-        width-wise border of the frame
+        Compute the global location of the center of the current image in the
+        map. You do not need to edit this method.
+
+        :param kp1: captured keyPoints
+        :param des1: captured descriptions
+        :param kp2: map keyPoints
+        :param des2: map descriptions
+        :return: global pose
         """
-        self.perceptual_range = self.pixel_to_meter(CAMERA_WIDTH / 2)
 
-    def resample_particles(self):
-        """
-        resample particles according to their weight
-
-        :param particles: the set of particles to resample
-        :return: a new set of particles, resampled with replacement according to their weight
-        """
+        good = []
+        pose = None
 
-        weight_sum = 0.0
-        new_particles = []
-        normal_weights = np.array([])
+        if des1 is not None and des2 is not None:
+            matches = self.matcher.knnMatch(des1, des2, k=2)
 
-        weights = [p.weight for p in self.particles]
-        lowest_weight = min(weights)
+            for match in matches:
+                if len(match) > 1 and match[0].distance < MATCH_RATIO * match[1].distance:
+                    good.append(match[0])
 
-        for w in weights:
-            x = 1 - (w / lowest_weight)
-            if x == 0:
-                x = PROB_THRESHOLD
-            normal_weights = np.append(normal_weights, x)
-            weight_sum += x
+            if len(good) > MIN_MATCH_COUNT:
+                src_pts = np.float32([kp1[m.queryIdx].pt for m in good]).reshape(-1, 1, 2)
+                dst_pts = np.float32([kp2[m.trainIdx].pt for m in good]).reshape(-1, 1, 2)
+                transform = cv2.estimateRigidTransform(src_pts, dst_pts, False)
+                if transform is not None:
+                    transformed_center = cv2.transform(CAMERA_CENTER, transform)  # get global pixel
+                    transformed_center = [transformed_center[0][0][0] / METER_TO_PIXEL,  # map to global pose
+                                          (MAP_PIXEL_HEIGHT - 1 - transformed_center[0][0][1]) / METER_TO_PIXEL]
+                    yaw = np.arctan2(transform[1, 0], transform[0, 0])  # get global heading
 
-        normal_weights /= weight_sum
-        samples = np.random.multinomial(self.num_particles, normal_weights)
-        for i, count in enumerate(samples):
-            for _ in range(count):
-                new_particles.append(copy.deepcopy(self.particles[i]))
+                    # correct the pose if the drone is not level
+                    z = math.sqrt(self.z ** 2 / (1 + math.tan(self.angle_x) ** 2 + math.tan(self.angle_y) ** 2))
+                    offset_x = np.tan(self.angle_x) * z
+                    offset_y = np.tan(self.angle_y) * z
+                    global_offset_x = math.cos(yaw) * offset_x + math.sin(yaw) * offset_y
+                    global_offset_y = math.sin(yaw) * offset_x + math.cos(yaw) * offset_y
+                    pose = [transformed_center[0] + global_offset_x, transformed_center[1] + global_offset_y, z, yaw]
 
-        self.particles = new_particles
+        return pose, len(good)
 
 
-def scale_weight(match0, match1):
-    """
-    uses the distances of the two best matches to provide a weight scaled between 0 and 1
+    def pixel_to_meter(self, px):
+        """
+        Uses the CAMERA_SCALE to return the pixel measurement converted into
+        meters. Note that this requires knowledge of the height of the drone.
+        You should use your code from the OpenCV assignment to implement this
+        method.
 
-    :param match0: the hamming distance of the first best match
-    :param match1: the hamming distance of the second best match
-    """
-    scaled = (match1 - match0) / float(match1)
-    if scaled == 0:
-        scaled = PROB_THRESHOLD
-    return scaled
+        :param px: the pixel measurement to convert to meters
+        :return: the meter equivalent of the pixel distance
+        """
 
+        #######################################################################
+        # TODO fill in this method with your code from the OpenCV task
+        #######################################################################
 
-def estimate_pose(particles):
+def adjust_angle(angle):
     """
-    retrieves the drone's estimated position by summing each particle's pose estimate multiplied
-    by its weight
+    Returns angle, ensuring that it stays within the range -pi to pi
 
-    Some mathematical motivation: imagine that the pose estimate of the drone along each degree of 
-    freedom is represented by a random variable. We find the pose estimate by taking the expectation
-    of the random variable:
-    Expectation[X] = sum over all x in X of p(x) * x
-    where p(x) is the weight of a particle and x is the pose of a particle
-
-    :param particles: the set of particles to estimate a position for
-    :return: the estimated pose [x,y,z,yaw]
+    :param angle: the angle to keep within -pi and pi
+    :return: angle adjusted to stay within the range -pi to pi
     """
+    while angle > math.pi:
+        angle -= 2 * math.pi
+    while angle <= -math.pi:
+        angle += 2 * math.pi
 
-    ##############################################################################
-    # TODO return the [x,y,z,yaw] pose estimate of the robot, you may reuse some or all of 
-    #      your localization code to do this
-    ##############################################################################
+    return angle
 
 
-    return [x, y, z, yaw]
+def create_map(file_name):
+    """
+    Creates a feature map by extracting features from the map image and sorting
+    them intro grids. You do not need to edit this method.
 
+    :param file_name: the image of the map
+    :return: the map_grid_des and map_grid_kp, two dimensional lists of the
+             kp and des from the map which are grouped into cells
+    """
 
+    # read image and extract features
+    image = cv2.imread(file_name)
+    # the edgeThreshold and patchSize can be tuned if the gap between cell is too large
+    detector = cv2.ORB(nfeatures=MAP_FEATURES, scoreType=cv2.ORB_FAST_SCORE)
+    max_total_keypoints = 500 * ORB_GRID_SIZE_X * ORB_GRID_SIZE_Y
+    detector_grid = cv2.GridAdaptedFeatureDetector(detector, maxTotalKeypoints=max_total_keypoints,
+                                                   gridCols=ORB_GRID_SIZE_X, gridRows=ORB_GRID_SIZE_Y)
+    kp = detector_grid.detect(image, None)
+    kp, des = detector.compute(image, kp)
+
+    # rearrange kp and des into grid
+    grid_kp = [[[] for _ in range(MAP_GRID_SIZE_Y)] for _ in range(MAP_GRID_SIZE_X)]
+    grid_des = [[[] for _ in range(MAP_GRID_SIZE_Y)] for _ in range(MAP_GRID_SIZE_X)]
+    for i in range(len(kp)):
+        x = int(kp[i].pt[0] / CELL_X)
+        y = MAP_GRID_SIZE_Y - 1 - int(kp[i].pt[1] / CELL_Y)
+        grid_kp[x][y].append(kp[i])
+        grid_des[x][y].append(des[i])
+
+    # group every 3 by 3 grid, so we can access each center of grid and its 8 neighbors easily
+    map_grid_kp = [[[] for _ in range(MAP_GRID_SIZE_Y)] for _ in range(MAP_GRID_SIZE_X)]
+    map_grid_des = [[[] for _ in range(MAP_GRID_SIZE_Y)] for _ in range(MAP_GRID_SIZE_X)]
+    for i in range(MAP_GRID_SIZE_X):
+        for j in range(MAP_GRID_SIZE_Y):
+            for k in range(-1, 2):
+                for l in range(-1, 2):
+                    x = i + k
+                    y = j + l
+                    if 0 <= x < MAP_GRID_SIZE_X and 0 <= y < MAP_GRID_SIZE_Y:
+                        map_grid_kp[i][j].extend(grid_kp[x][y])
+                        map_grid_des[i][j].extend(grid_des[x][y])
+            map_grid_des[i][j] = np.array(map_grid_des[i][j])
+
+    return map_grid_kp, map_grid_des
+
+
+def norm_pdf(x, mu, sigma):
+    """
+    Finds the probability of obtaining the sample x from a normal distribution
+    with mean mu and variance sigma. You do not need to edit this method.
+
+    :param x: the sample to find the probability of
+    :param mu: the mean of the normal distribution
+    :param sigma: the variance of the normal distribution
+    :return: the probability of obtaining the sample x from a Gaussian with
+             mean mu and variance sigma
+    """
 
+    u = (x - mu) / float(abs(sigma))
+    y = (1 / (np.sqrt(2 * np.pi) * abs(sigma))) * np.exp(-u * u / 2.)
+    return y