无人驾驶6：马尔卡夫滤波

无人车定位问题

准确定位，是无人车技术的基础，常用的GPS定位，误差经常为2_{10m，而无人车的精度要求2}10cm左右，怎么实现呢，这就是无人车的定位问题。

变量定义

$z_{1:t}$z1:t​: 从时间步骤1到t的所有观测， 观察数据可能是距离测量值，方向角或者图像等等。

$u_{1:t}$u1:t​: 从时间步骤1到t的所有控制元素，一般包括偏航角、间距或滚动率、速度信息

m: 可能是全球环境的网格地图，或一个包括全球特征点和车道几何图形的数据库；

知道车辆的本地坐标系和地图的全球坐标系之间的转换，就可以知道车辆在全球的位置；

$x_t$xt​: 车辆在时间t的位置，包括坐标(x,y)，方向 $\theta$θ;

定位就是估算状态$x_t$xt​, 也就是车辆位置；

贝叶斯公式

With respect to localization, these terms are:

P(location|observation): This is P(a|b), the normalized probability of a position given an observation (posterior).

P(observation|location): This is P(b|a), the probability of an observation given a position (likelihood)

P(location): This is P(a), the prior probability of a position

P(observation): This is P(b), the total probability of an observation

后验概率的定义：

$bel(x_t) = p(x_t|z_{1:t}, u_{1:t}, m)$bel(xt​)=p(xt​∣z1:t​,u1:t​,m)

求得后验概率的前提条件是，所有的之前观察（$z_{1:t}$z1:t​），所有的控制($u_{1:t}$u1:t​), 假设地图已知且不变(地图变，就是所谓SLAM问题了)。

由后验概率定义看出，需要处理大量的数据(测量数据大量，且累计)，因此不可行；

1.马尔可夫定位

核心思想： 我们不想利用全部观察历史来估算当前状态，尝试从$bel(x_t) = p(x_t|z_{1:t}, u_{1:t}, m)$bel(xt​)=p(xt​∣z1:t​,u1:t​,m)得到递归状态的估算器，

当前信仰，可以用上一个状态和新的观察信息估算得到。这个估算叫做贝叶斯定位滤波器，或者马尔卡夫定位。

这样就不用加载所有历史的观察和运动数据，要获得这个递归公式，必须运动贝叶斯公式，全概率公式，马尔可夫假设。

P(location)是由运动模型和前一个状态决定的

寻找递归公式

Bayes Rule

$P(a \mid b) = \frac{P(b \mid a) \, P(a)}{P(b)}$P(a∣b)=P(b)P(b∣a)P(a)​

重新定义测量数据，对应到当前模型

$p(x_t∣z_t, z_{ 1:t−1}, u _{1:t}, m) = \frac{p(z_t|x_t,z_{1:t-1},u_{1:t},m)p(x_t|z_{1:t-1}, u_{1:t}, m)}{p(z_t|z_{1:t-1},u_{1:t},m)}$p(xt​∣zt​,z1:t−1​,u1:t​,m)=p(zt​∣z1:t−1​,u1:t​,m)p(zt​∣xt​,z1:t−1​,u1:t​,m)p(xt​∣z1:t−1​,u1:t​,m)​

马尔卡夫假设

通用的计算模型

马尔可夫假设：$\underline{假设上一个状态的x_{t-1}估算非常理想，其包括了对之前的所有因素总和估计，那么当前状态x_t可以忽略掉t-1时刻之前历史数据，只用x_{t-1}和u_t,m来估算。}$假设上一个状态的xt−1​估算非常理想，其包括了对之前的所有因素总和估计，那么当前状态xt​可以忽略掉t−1时刻之前历史数据，只用xt−1​和ut​,m来估算。​

$p(x_t) = x_{t-1} + motion_{model}$p(xt​)=xt−1​+motionmodel​

简略改写为：

注：对于离散模型，积分用加法代替。

Reference Equations

Discretized Motion Model:
$\sum\limits_{i} p(x_t|x_{t-1}^{(i)}, u_t, m)bel(x_{t-1}^{(i)})$i∑​p(xt​∣xt−1(i)​,ut​,m)bel(xt−1(i)​)

Transition Model:
$p(x_t|x_{t-1}^{(i)}, u_t, m)$p(xt​∣xt−1(i)​,ut​,m)

'i’th Motion Model Probability:
$p(x_t|x_{t-1}^{(i)} u_t, m) *bel(x_{t-1}^{(i)})$p(xt​∣xt−1(i)​ut​,m)∗bel(xt−1(i)​)

以上是运动模型的数学原理，代码实现：

// TODO: implement the motion model: calculates prob of being at 
// an estimated position at time t
float motion_model(float pseudo_position, float movement, vector<float> priors,
                   int map_size, int control_stdev) {
  // initialize probability
  float position_prob = 0.0f;
  
  // YOUR CODE HERE
  for (float j=0; j < map_size; j++)
  {
      float next_psududo_position = j;
      float distance_ij =  pseudo_position - next_psududo_position;
      float transition_prob = Helpers::normpdf(distance_ij,movement,control_stdev);
      
      position_prob += transition_prob*priors[(int)j];
  }

  
  return position_prob;
}

// step through each pseudo position x (i)    
  for (float i = 0; i < map_size; ++i) {
    float pseudo_position = i;

    // get the motion model probability for each x position
    float motion_prob = motion_model(pseudo_position, movement_per_timestep,
                                     priors, map_size, control_stdev);
        
    // print to stdout
    std::cout << pseudo_position << "\t" << motion_prob << std::endl;
  }    

Bayes Filter for Localization (Markov Localization)

$bel(x_t) = p(x_t|z_t,z_{1:t-1},\mu_{1:t},m) = \eta *p(z_t|x_t,m) \hat{bel}(x_t)$bel(xt​)=p(xt​∣zt​,z1:t−1​,μ1:t​,m)=η∗p(zt​∣xt​,m)bel^(xt​)

2.测量更新

测量模型如下：

同样运用马尔可夫假设，简化模型如下：

注意：$\underline{一个时刻t, 依次测量，会测量多个地图上的物体对象，每单个物体测量都是相互独立的。}$一个时刻t,依次测量，会测量多个地图上的物体对象，每单个物体测量都是相互独立的。​

如何定义单个距离测量值的观察模型呢？

一般来说，车辆有很多不同的传感器，比如激光雷达，毫米波雷达，摄像头，超声传感器等，
每个传感器都有各自的噪声行为和表现，观察模型还与地图有关，比如一维地图上，距离测量模型为高斯分布，测量如下：

马尔可夫定位的贝叶斯公式

贝叶斯滤波器

通用的贝叶斯滤波器，包含预测和测量两部分，完整的贝叶斯滤波器，就是预测和测量的循环更新。

马尔可夫滤波，卡尔曼滤波，直方图滤波，都属于贝叶斯滤波器。

代码实现：

//helpers.h

#ifndef HELP_FUNCTIONS_H
#define HELP_FUNCTIONS_H

#include <math.h>

class Helpers {
 public:
  // definition of one over square root of 2*pi:
  constexpr static float STATIC_ONE_OVER_SQRT_2PI = 1/sqrt(2*M_PI);

  /**
   * normpdf(X,mu,sigma) computes the probability function at values x using the
   * normal distribution with mean mu and standard deviation std. x, mu and 
   * sigma must be scalar! The parameter std must be positive. 
   * The normal pdf is y=f(x,mu,std)= 1/(std*sqrt(2pi)) e[ -(x−mu)^2 / 2*std^2 ]
   */
  static float normpdf(float x, float mu, float std) {
    return (STATIC_ONE_OVER_SQRT_2PI/std)*exp(-0.5*pow((x-mu)/std,2));
  }

  // static function to normalize a vector
  static std::vector<float> normalize_vector(std::vector<float> inputVector) {

    // declare sum 
    float sum = 0.0f;

    // declare and resize output vector
    std::vector<float> outputVector;
    outputVector.resize(inputVector.size());

    // estimate the sum
    for (int i = 0; i < inputVector.size(); ++i) {
      sum += inputVector[i];
    }

    // normalize with sum
    for (int i = 0; i < inputVector.size(); ++i) {
      outputVector[i] = inputVector[i]/sum;
    }

    // return normalized vector:
    return outputVector;
  }
};

#endif  // HELP_FUNCTIONS_H

main.cpp

#include <algorithm>
#include <iostream>
#include <vector>

#include "helpers.h"

using std::vector;
using std::cout;
using std::endl;


vector<float> initialize_priors(int map_size, vector<float> landmark_positions,
                                float position_stdev);

float motion_model(float pseudo_position, float movement, vector<float> priors,
                   int map_size, int control_stdev);

// function to get pseudo ranges
vector<float> pseudo_range_estimator(vector<float> landmark_positions, 
                                     float pseudo_position);

// observation model: calculate likelihood prob term based on landmark proximity
float observation_model(vector<float> landmark_positions, 
                        vector<float> observations, vector<float> pseudo_ranges,
                        float distance_max, float observation_stdev);


int main() {  
  // set standard deviation of control
  float control_stdev = 1.0f;

  // set standard deviation of position
  float position_stdev = 1.0f;

  // meters vehicle moves per time step
  float movement_per_timestep = 1.0f;

  // set observation standard deviation
  float observation_stdev = 1.0f;

  // number of x positions on map
  int map_size = 25;

  // set distance max
  float distance_max = map_size;

  // define landmarks
  vector<float> landmark_positions {3, 9, 14, 23};

  // define observations vector, each inner vector represents a set 
  //   of observations for a time step
  vector<vector<float> > sensor_obs {{1,7,12,21}, {0,6,11,20}, {5,10,19},
                                     {4,9,18}, {3,8,17}, {2,7,16}, {1,6,15}, 
                                     {0,5,14}, {4,13}, {3,12}, {2,11}, {1,10},
                                     {0,9}, {8}, {7}, {6}, {5}, {4}, {3}, {2},
                                     {1}, {0}, {}, {}, {}};

  /**
   * TODO: initialize priors
   */
   //初始先验概率，每一个完整滤波周期都会更新priors
  vector <float> priors = initialize_priors(map_size,landmark_positions,position_stdev);

  // UNCOMMENT TO SEE THIS STEP OF THE FILTER
  cout << "-----------PRIORS INIT--------------" << endl;
  for (int p = 0; p < priors.size(); ++p){
    cout << priors[p] << endl;
  }  
  cout << "print priors ends."<<endl;
  // initialize posteriors
  vector<float> posteriors(map_size, 0.0);

  // specify time steps
  int time_steps = sensor_obs.size();
    
  // declare observations vector
  vector<float> observations;
    
  // cycle through time steps
  for (int t = 0; t < time_steps; ++t) {
    // UNCOMMENT TO SEE THIS STEP OF THE FILTER
    cout << "---------------TIME STEP---------------" << endl;
    cout << "t = " << t << endl;
    cout << "-----Motion----------OBS----------------PRODUCT--" << endl;

    if (!sensor_obs[t].empty()) {
      observations = sensor_obs[t]; 
    } else {
      observations = {float(distance_max)};
    }

    // step through each pseudo position x (i)
    for (unsigned int i = 0; i < map_size; ++i) {
      float pseudo_position = float(i);

      /**
       * TODO: get the motion model probability for each x position
       */
      //全概率公式
      float motion_prob = motion_model(pseudo_position,movement_per_timestep,priors,map_size,control_stdev);

      /**
       * TODO: get pseudo ranges
       */
      //获得伪距离序列
      vector<float> pseudo_ranges = pseudo_range_estimator(landmark_positions,pseudo_position);

      /**
       * TODO: get observation probability
       */
      //测量更新
      float observation_prob = observation_model(landmark_positions,observations,pseudo_ranges,distance_max,observation_stdev);

      /**
       * TODO: calculate the ith posterior and pass to posteriors vector
       */
      posteriors[i] = motion_prob*observation_prob;

      // UNCOMMENT TO SEE THIS STEP OF THE FILTER
      cout << motion_prob << "\t" << observation_prob << "\t" 
           << "\t"  << motion_prob * observation_prob << endl;   
    } 
        
    // UNCOMMENT TO SEE THIS STEP OF THE FILTER
    cout << "----------RAW---------------" << endl;
    for (int p = 0; p < posteriors.size(); ++p) {
      cout << posteriors[p] << endl;
    }

    /**
     * TODO: normalize posteriors (see helpers.h for a helper function)
     */
    
    posteriors = Helpers::normalize_vector(posteriors);
    // print to stdout
    //cout << posteriors[t] <<  "\t" << priors[t] << endl;

    // UNCOMMENT TO SEE THIS STEP OF THE FILTER
    cout << "----------NORMALIZED---------------" << endl;

    /**
     * TODO: update priors
     */
    priors = posteriors;

    // UNCOMMENT TO SEE THIS STEP OF THE FILTER
    //for (int p = 0; p < posteriors.size(); ++p) {
    //  cout << posteriors[p] << endl;
    //}

    // print posteriors vectors to stdout
    for (int p = 0; p < posteriors.size(); ++p) {
            cout << posteriors[p] << endl;  
    } 
  }

  return 0;
}

// observation model: calculate likelihood prob term based on landmark proximity
float observation_model(vector<float> landmark_positions, 
                        vector<float> observations, vector<float> pseudo_ranges, 
                        float distance_max, float observation_stdev) {
  // initialize observation probability
  float distance_prob = 1.0f;

  // run over current observation vector
  for (int z=0; z< observations.size(); ++z) {
    // define min distance
    float pseudo_range_min;
        
    // check, if distance vector exists
    if (pseudo_ranges.size() > 0) {
      // set min distance
      pseudo_range_min = pseudo_ranges[0];
      // remove this entry from pseudo_ranges-vector
      pseudo_ranges.erase(pseudo_ranges.begin());
    } else {  // no or negative distances: set min distance to a large number
        pseudo_range_min = std::numeric_limits<const float>::infinity();
    }

    // estimate the probability for observation model, this is our likelihood 
    distance_prob *= Helpers::normpdf(observations[z], pseudo_range_min,
                                      observation_stdev);
  }

  return distance_prob;
}

vector<float> pseudo_range_estimator(vector<float> landmark_positions, 
                                     float pseudo_position) {
  // define pseudo observation vector
  vector<float> pseudo_ranges;
            
  // loop over number of landmarks and estimate pseudo ranges
  for (int l=0; l< landmark_positions.size(); ++l) {
    // estimate pseudo range for each single landmark 
    // and the current state position pose_i:
    float range_l = landmark_positions[l] - pseudo_position;

    // check if distances are positive: 
    if (range_l > 0.0f) {
      pseudo_ranges.push_back(range_l);
    }
  }

  // sort pseudo range vector
  sort(pseudo_ranges.begin(), pseudo_ranges.end());

  return pseudo_ranges;
}

// motion model: calculates prob of being at an estimated position at time t
float motion_model(float pseudo_position, float movement, vector<float> priors,
                   int map_size, int control_stdev) {
  // initialize probability
  float position_prob = 0.0f;

  // loop over state space for all possible positions x (convolution):
  for (float j=0; j< map_size; ++j) {
    float next_pseudo_position = j;
    // distance from i to j
    float distance_ij = pseudo_position-next_pseudo_position;

    // transition probabilities:
    float transition_prob = Helpers::normpdf(distance_ij, movement, 
                                             control_stdev);
    // estimate probability for the motion model, this is our prior
    position_prob += transition_prob*priors[j];
  }

  return position_prob;
}

// initialize priors assuming vehicle at landmark +/- 1.0 meters position stdev
vector<float> initialize_priors(int map_size, vector<float> landmark_positions,
                                     float position_stdev) {
  // set all priors to 0.0
  vector<float> priors(map_size, 0.0);

  // set each landmark positon +/-1 to 1.0/9.0 (9 possible postions)
  float norm_term = landmark_positions.size() * (position_stdev * 2 + 1);
  for (int i=0; i < landmark_positions.size(); ++i) {
    for (float j=1; j <= position_stdev; ++j) {
      priors.at(int(j+landmark_positions[i]+map_size)%map_size) += 1.0/norm_term;
      priors.at(int(-j+landmark_positions[i]+map_size)%map_size) += 1.0/norm_term;
    }
    priors.at(landmark_positions[i]) += 1.0/norm_term;
  }

  return priors;
}