KLD采样|激光SLAMamcl和粒子滤波

根据KLD采样计算定位所需的粒子数量，不需要重采样则对粒子集进行排序，选出权重最大的cluster，其位姿为该时刻机器人的最优估计位姿。

// Compute the required number of samples, given that there are k bins
// with samples in them. This is taken directly from Fox et al.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
int pf_resample_limit(pf_t *pf, int k)
{
  double a, b, c, x;
  int n;
  if (k <= 1)
    return pf->max_samples;
  a = 1;
  b = 2 / (9 * ((double) k - 1));
  // pop_z 为3
  c = sqrt(2 / (9 * ((double) k - 1))) * pf->pop_z;
  x = a - b + c;
  // pop_err 为0.01
  n = (int) ceil(   (k - 1) / (2 * pf->pop_err) * x * x * x   );

  if (n < pf->min_samples)
    return pf->min_samples;
  if (n > pf->max_samples)
    return pf->max_samples;
  
  return n;
}

粒子收敛，我们在rviz上看到的particlecloud话题，它的元素个数，粒子数其实是重采样之后的粒子集的粒子数set_b：

1
2
3
4
5
6
7
if(  !(++resample_count_ % resample_interval_)  )
{
  pf_update_resample(pf_);
  resampled = true;
}
pf_sample_set_t* set = pf_->sets + pf_->current_set;
ROS_DEBUG("Num samples: %d\n", set->sample_count);

开始为max_particles，开始滤波后，虽然rviz上粒子看上去收敛到一起，但是粒子数不一定只有那么少。

如果min_particles大于pf_resample_limit计算得到的粒子数，那么particlecloud对应得粒子数是min_particles；如果小于，就是后者。也就是取二者的最大值。 但这个函数返回值并不是单调增或单调

Matlab读文件和写文件|Matlab

读文件

txt文件里是这样的：

1
2
3
4
-3.35 2.332 
-3.18 1.876 
-3.10 2.577 
-3.51 2.991

可以这样画散点图，最后设置了坐标轴的范围：

1
2
3
4
5
6
clear all
close all
data=load('E:\data.txt');
scatter(data(:,1),data(:,2))
xlim([-6 4])
ylim([-4 10])

散点图.png

读点云文件画图：

1
2
3
4
5
6
7
8
9
10
11
A = load('cloud3d.dat'); %读入数据
%x,y,z轴坐标
x=A(:,1);
y=A(:,2);
z=A(:,3);
scatter3(x,y,z);    %散点图
[X,Y,Z]=griddata(x,y,z,linspace(min(x),max(x))',linspace(min(y),max(y)),'v4'); %构造坐标点
pcolor(X,Y,Z);
shading interp;        %伪彩色图
fcontourf(X,Y,Z);     %等高线图
figure,surf(X,Y,Z);    %三维曲面

写文件

1
2
3
4
5
6
7
8
fileID = fopen('F:\test.ply', 'w');
fprintf(fileID, 'ply\n');
fprintf(fileID, 'format ascii 1.0\n');
fprintf(fileID, 'property int path_id\n');
fprintf(fileID, 'property int group_id\n');
fprintf(fileID, 'end_header\n');
fprintf(fileID, '%f %f %f %d %d\n', [1,2,3,4,5]);
fclose(fileID);

打开文件是：

1
2
3
4
5
6
ply
format ascii 1.0
property int path_id
property int group_id
end_header
1.000000 2.000000 3.000000 4 5

pf_init 函数|激光SLAMamcl和粒子滤波

原型：void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)

pf是之前初始化的粒子滤波器对象，mean是amcl中的初始位姿[x,y,θ], cov是初始协方差

1
2
3
4
5
6
7
8
9
// 清空kd树
void pf_kdtree_clear(pf_kdtree_t *self)
{
  self->root = NULL;
  self->leaf_count = 0;
  self->node_count = 0;

  return;
}

调用 pf_cluster_stats的函数

pf_cluster_stats的调用关系.png
源码里没有用到pf_init_model，它完成的功能与pf_init基本是一样的，只是pf_init使用的是高斯模型，pf_init_model可以由用户提供模型。用户可以定义一个service客户端，发起global_localization服务，AMCL源码已经定义好了回调函数。

pf_init_model有三个参数，其中pf仍然是滤波器对象。init_fn是一个函数指针，用户需要提供一个生成随机样本的函数实现，该参数的作用就相当于pf_init中的高斯分布对象。参数init_data则是生成粒子的样本空间， 我们可以为之赋予地图数据。

pf_alloc函数|激光SLAMamcl和粒子滤波

先看函数流程图：pf_alloc.png

源码分析(二) handleMapMessage 函数|激光SLAMamcl和粒子滤波

调用关系.png
实际用的是requstMap这一支

准备工作

参数是const nav_msgs::OccupancyGrid& msg

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
// 地图的宽高，分辨率
ROS_INFO("Received a %d X %d map @ %.3f m/pix\n",
         msg.info.width,
         msg.info.height,
         msg.info.resolution);
// 如果目前的全局坐标系不是map
if(msg.header.frame_id != global_frame_id_)
  ROS_WARN("Frame_id of map received:'%s' doesn't match global_frame_id:'%s'. This could cause issues with reading published topics",
           msg.header.frame_id.c_str(),
           global_frame_id_.c_str()  );

//free相应的指针
freeMapDependentMemory();
// Clear queued laser objects because they hold pointers to the existing map
lasers_.clear();
lasers_update_.clear();
frame_to_laser_.clear();

处理获得的地图

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// AMCL所用的地图类型
typedef struct
{
  // Map origin; the map is a viewport onto a conceptual larger map.
  double origin_x, origin_y;
  
  double scale;  // Map scale (m/cell)

  // Map dimensions (number of cells)
  int size_x, size_y;
  
  // The map data, stored as a grid
  map_cell_t *cells;

  // Max distance at which we care about obstacles, for constructing
  // likelihood field
  double max_occ_dist;
} map_t;

转换为AMCL的地图信息

1
2
3
4
5
6
7
8
9
//转换成AMCL的标准地图，0->-1(不是障碍);100->+1（障碍）;else->0（不明）
map_ = convertMap(msg);  // 类型map_t

//将不是障碍的点的坐标保存下来, 构成地图栅格的空闲区域
free_space_indices.resize(0);
for(int i = 0; i < map_->size_x; i++)
  for(int j = 0; j < map_->size_y; j++)
    if(map_->cells[MAP_INDEX(map_,i,j)].occ_state == -1)
      free_space_indices.push_back(std::make_pair(i,j));

从参数服务器获取初始位姿及方差放到pf中

1
updatePoseFromServer();

其实就是rosparam把6个参数放到init_pose_和init_cov_两个数组中，供下面进一步赋值

创建粒子滤波器并初始化

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
pf_ = pf_alloc(min_particles_, max_particles_,
               alpha_slow_, alpha_fast_,
               (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
               (void *)map_);
pf_->pop_err = pf_err_;
pf_->pop_z = pf_z_;

pf_vector_t pf_init_pose_mean = pf_vector_zero();
pf_init_pose_mean.v[0] = init_pose_[0];
pf_init_pose_mean.v[1] = init_pose_[1];
pf_init_pose_mean.v[2] = init_pose_[2];
pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
pf_init_pose_cov.m[0][0] = init_cov_[0];
pf_init_pose_cov.m[1][1] = init_cov_[1];
pf_init_pose_cov.m[2][2] = init_cov_[2];

pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
pf_init_ = false;

pf_alloc和pf_init是两个重要函数。 pf_alloc只在这里调用了一次，pf_init在这里和applyInitialPose里调用

专门分析pf_alloc

专门分析pf_init

定义里程计与激光雷达并初始化数据

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
delete odom_;
odom_ = new AMCLOdom();
ROS_ASSERT(odom_);
// 设置里程计参数
odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );

// Laser
delete laser_;
// max_beams_就是参数laser_max_beams
// map_ 是AMCL所用的地图格式, 在上面获得
laser_ = new AMCLLaser(max_beams_, map_);
ROS_ASSERT(laser_);
// 4个形参为似然域模型的参数
laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
                                  laser_likelihood_max_dist_);

SetModelLikelihoodField函数就是使用前三个形参用于赋值, 然后调用 map_update_cspace(this->map, laser_likelihood_max_dist_);， 这个是似然域模型中最复杂的函数
它的专门分析在这里

参数laser_likelihood_max_dist默认为2，是地图上做障碍物膨胀的最大距离，用于似然域模型

这几个参数没有用在似然域模型: laser_z_short, laser_z_max, laser_lambda_short, laser_likelihood_max_dist

applyInitialPose()

// In case the initial pose message arrived before the first map,
// try to apply the initial pose now that the map has arrived.
可能初始位姿的信息比第一个map先到达，map到达后apply初始位姿

1
2
3
4
5
6
7
8
boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
if( initial_pose_hyp_ != NULL && map_ != NULL ) {
  pf_init(pf_, initial_pose_hyp_->pf_pose_mean, initial_pose_hyp_->pf_pose_cov);
  pf_init_ = false;

  delete initial_pose_hyp_;
  initial_pose_hyp_ = NULL;
}

函数指针|C++C++ 基础

函数名就是个指针,它指向函数代码在内存中的首地址.

1
2
3
4
5
6
7
8
9
typedef double (*FuncType) (int data);
double func(int num)
{
    return num;
}

// 调用
FuncType ptr = func;
cout << ptr(11) <<endl;

第一行定义了一种函数指针,类型为FuncType,它指向的函数返回为 double, 形参为一个int. 然后定义了一个名为func的函数.

FuncType ptr = func;是声明一个指针, 类型为 FuncType, 指向func函数. 接下来就可以拿ptr当函数用了

1
2
3
4
5
6
7
8
void test(int id, FuncType foo)
{
    cout << foo(110) << endl;
    cout << "id: "<<id<<endl;
}

//调用
test(1, static_cast<FuncType>(func) );

定义一个函数test, 它第二个形参是类型为FuncType的函数指针. 调用时, 最好对第二个形参做转换, 这里用static_cast再合适不过.

源码分析(三)laserReceived回调函数|激光SLAMamcl和粒子滤波

这是amcl_node.cpp中最重要的函数，蒙特卡洛算法都在它里面，我们前面分析几个算法时已经涉及到了它的几个部分，下面再串起来看一遍

laserReceived 回调函数

Amcl的业务逻辑总体就是在一个四五百行的巨大函数laserReceived中实现的，它就是刚刚我们提到的消息过滤器laser_scan_filter_的回调函数。

  • 用变量lastlaser_received_ts记录下当前的系统时间， 它用于判定是否长时间未接收到雷达数据。此外如果没有地图对象，将直接退出

  • AmclNode借助一些vector和map的容器，来支持多传感器。它使用lasers记录下当前构建的雷达对象，lasers_update标记雷达的更新状态， 并通过一个string到int的map建立其雷达坐标ID到雷达对象在lasers_中的对应关系。

  • 通过在frameto_laser中查找雷达的坐标ID，如果之前没有收到该雷达的消息，将新建一个对象保存在lasers中，并相应的在lasers_update中添加对应更新状态， 建立映射关系。否则，我们就直接通过frameto_laser获取雷达对象的索引。

  • 利用tf求出雷达坐标系的原点在base_link下的坐标，然后调用SetLaserPose设置雷达的位姿，angle暂时为0

  • getOdomPose获取收到雷达数据时的里程计位姿

  • if(pf_init_)的内容一定执行，如果里程计的数据显示机器人已经发生了明显的位移或者旋转，就标记所有的雷达更新标记为true

  • 根据刚刚更新标识，我们调用里程计对象的UpdateAction接口完成运动更新，也就是机器人移动后执行else if(pf_init_ && lasers_update_[laser_index])的内容

  • 机器人移动，执行if(lasers_update_[laser_index])的内容，根据激光的扫描数据更新滤波器。首先构建AMCLLaserData对象，并指定传感器对象和量程，然后将接收到的传感器数据拷贝到ldata对象中，最后通过激光传感器对象的UpdateSensor接口完成粒子滤波器的测量更新。这里发布了particlecloud话题

  • 机器人移动，则执行if(resampled || force_publication)的内容，发布了amcl_pose话题

AmclNode::getOdomPose维护odom—>base_link的变换

map—>odom的tf变换在1410行左右：

1
2
3
4
5
6
7
ros::Time transform_expiration = (laser_scan->header.stamp + transform_tolerance_);
//  构造一个StampedTransform 需要tf::Transform ,stamp_,father_frame,child_frame 参数
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),// odom 到map的转换矩阵
                                    transform_expiration,
                                    global_frame_id_, odom_frame_id_);

this->tfb_->sendTransform(tmp_tf_stamped);//发布 odom 到map的转换矩阵转换

源码分析(五)测量模型|激光SLAMamcl和粒子滤波

测量模型主要用来做scan-match。

光束模型比较古老，实际已经不怎么使用，它的缺陷比较明显：在杂乱的环境(非结构化环境clutter)中，输入的小变化会导致输出的大变化。amcl中实现了这个模型，但是可以选择用还是不用

似然场模型不仅适应于结构化和非结构化的环境，而且在任何环境中的期望值对位姿都是平滑的，还可以通过查表的得到栅格的似然度。

工具函数和类

1
2
3
4
5
6
7
8
9
10
11
// Transform from local to global coords (a + b)
pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b)
{
  pf_vector_t c;

  c.v[0] = b.v[0] + a.v[0] * cos(b.v[2]) - a.v[1] * sin(b.v[2]);
  c.v[1] = b.v[1] + a.v[0] * sin(b.v[2]) + a.v[1] * cos(b.v[2]);
  c.v[2] = b.v[2] + a.v[2];
  c.v[2] = atan2(sin(c.v[2]), cos(c.v[2]));
  return c;
}

laserReceived 中的准备工作

这一段是紧接着laserReceived中运动模型的准备工作

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
// 刚启动amcl时不执行,机器人移动后执行, update the filter
if(lasers_update_[laser_index])   // 在运动模型的准备工作里赋值true
{
    AMCLLaserData ldata;
    // 成员sensor是指向传感器的指针
    ldata.sensor = lasers_[laser_index];
    // 动态数组ranges的大小，可以用 rostopic echo --noarr /scan 获得
    ldata.range_count = laser_scan->ranges.size();

    
    // 雷达在base_link坐标系的最小和最大偏角
    tf::Quaternion q;
    // 获得雷达的扫描起始角度，例如思岚A2的是-3.124，带时间戳和坐标系名称
    q.setRPY(0.0, 0.0, laser_scan->angle_min);
    tf::Stamped<tf::Quaternion> min_q(q, laser_scan->header.stamp,
                                      laser_scan->header.frame_id);
    // angle_increment为角度分辨率，例如思岚A2的是0.017， inc_q: 起始角度+角度分辨率
    q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment为);
    tf::Stamped<tf::Quaternion> inc_q(q, laser_scan->header.stamp,
                                      laser_scan->header.frame_id);
    try
    {      // 获得两个角度在base_link坐标系中的Quaternion
      tf_->transformQuaternion(base_frame_id_, min_q, min_q);
      tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);
    }
    catch(tf::TransformException& e)
    {
      ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
               e.what());
      return;
    }
    // 现在是base_link中的起始yaw和偏航角分辨率，单位rad
    double angle_min = tf::getYaw(min_q);
    double angle_increment = tf::getYaw(inc_q) - angle_min;

    // 把角度映射到[-pi ~ pi]的范围
    angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI;
    ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment);


    // Apply range min/max thresholds, if the user supplied them
    // 来自参数 laser_max_range 和 laser_min_range， 准备后面使用
    if(laser_max_range_ > 0.0)
        ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
    else
        ldata.range_max = laser_scan->range_max;
    double range_min;
    if(laser_min_range_ > 0.0)
        range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
    else
        range_min = laser_scan->range_min;


    // 二维数组，AMCLLaserData 的析构函数会释放这段内存
    // 每行第1个元素是激光射程，第2个是相对机器人的夹角
    ldata.ranges = new double[ldata.range_count][2];
    ROS_ASSERT(ldata.ranges);
    for(int i=0;i<ldata.range_count;i++)
    {
      // amcl还没有最小射程的概念，如果range的值不正常，小于最小值，则映射为最大值 
      // 雷达数据的赋值
      if(laser_scan->ranges[i] <= range_min)
          ldata.ranges[i][0] = ldata.range_max;
      else   // 射程的正常赋值，来自话题scan
          ldata.ranges[i][0] = laser_scan->ranges[i];
          // Compute bearing 夹角的赋值
          ldata.ranges[i][1] = angle_min + (i * angle_increment);
    }

    // 核心函数， 更新传感器数据
    lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);
    lasers_update_[laser_index] = false;  // 完成更新传感器数据，标志位变为false

    pf_odom_pose_ = pose;

}


UpdateSensor函数的调用过程：

1
2
3
bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data)
// pf_sensor_model_fn_t是函数指针的类型
pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModel, data);

涉及两个函数： LikelihoodFieldModel 似然域模型，pf_update_sensor 粒子更新

LikelihoodFieldModel 函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set)
{
  AMCLLaser *self;
  int i, j, step;
  double z, pz;
  double p;
  double obs_range, obs_bearing;
  double total_weight;
  pf_sample_t *sample;
  pf_vector_t pose;
  pf_vector_t hit;

  self = (AMCLLaser*) data->sensor;
  total_weight = 0.0;

  // Compute the sample weights 遍历整个粒子集合，重新计算权重
  for (j = 0; j < set->sample_count; j++)
  {
    sample = set->samples + j;
    pose = sample->pose;

    // 传感器局部坐标经过三角变换映射到全局坐标系下，与激光雷达数据匹配
    pose = pf_vector_coord_add(self->laser_pose, pose);
    p = 1.0;

    // 预计算似然域，离散栅格化，《P.R》Page 130, 其实是对sigma和Zmax的赋值
    double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit; //高斯噪声的方差
    double z_rand_mult = 1.0/data->range_max;  // 随机噪声的分布

    //因为时间的问题，不对所有的点都进行似然计算，这里只是间隔地选点
    step = (data->range_count - 1) / (self->max_beams - 1);
    if(step < 1)  // 步长至少为1
      step = 1;

  	// 利用与最近物体的欧氏距离计算激光模型似然，对所有特征（激光点）进行遍历
    for (i = 0; i < data->range_count; i += step)
    {
      // 距离和角度的赋值，根源在上面的准备工作里
      obs_range = data->ranges[i][0];  
      obs_bearing = data->ranges[i][1];

      //忽略极大的range，rostopic echo可看到有很多range是inf
      if(obs_range >= data->range_max)
        continue;
      // Check for NaN
      if(obs_range != obs_range)
        continue;

      pz = 0.0;
      //  激光点通过两次转化到全局坐标系，  map  base_link  lase
      // Compute the endpoint of the beam
      hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
      hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);

      // 将激光数据在栅格地图中进行离散化, 获得激光点在栅格地图的坐标
      // 我理解的查表就是这里
      int mi, mj;
      mi = MAP_GXWX(self->map, hit.v[0]);
      // 计算函数在map_cspace.cpp的 map_update_cspace 中实现遍历计算
      // 该函数是被AMCLLaser::SetModelLikelihoodField调用
      mj = MAP_GYWY(self->map, hit.v[1]);  //距离和角度格栅
      
      // 找到距离激光点最近的障碍，距离值是提前计算好的
      if(!MAP_VALID(self->map, mi, mj))
        z = self->map->max_occ_dist;   //不在地图范围内的当成最大距离
      else
        // 这个就是我们需要的最近距离，相当于《P.R》130页的dist
        z = self->map->cells[MAP_INDEX(self->map, mi, mj)].occ_dist;


      // 高斯模型，130页的算法第8行
      // 计算Zt似然域的算法，在障碍物点周围用正态分布和均匀分布计算似然域，是关于dist的函数
      pz += self->z_hit * exp(-(z * z) / z_hit_denom);  //高斯测量
      pz += self->z_rand * z_rand_mult;   //随机测量

      // TODO: outlier rejection for short readings
      assert(pz <= 1.0);
      assert(pz >= 0.0);
      //  p *= pz;  这个是书上的公式
      // here we have an ad-hoc weighting scheme for combining beam probs
      // 注意： 实际使用的公式和书上的不同
      p += pz*pz*pz;  	//将每个激光点的似然域累加起来（多大范围）
    }
    // sample->weight 在之前的pf_alloc和pf_init里赋值为 最大粒子数的倒数
    sample->weight *= p;    // 概率等于权重
    total_weight += sample->weight;
  }
  return(total_weight);
}

大致的思想是这样:

不解的地方：

测量模型的最后使用的公式是p += pz*pz*pz，并不是书上的p *= pz，怀疑3个pz对应 [x,y,θ]

sample->weight在之前赋值为 1/max_particles 

1
2
3
// 将激光点的世界坐标转为栅格地图的坐标
#define MAP_GXWX(map, x) (floor( (x - map->origin_x) / map->scale + 0.5) + map->size_x / 2)
#define MAP_GYWY(map, y) (floor( (y - map->origin_y) / map->scale + 0.5) + map->size_y / 2)

公式 step = (laser_range_count - 1) / (max_beams - 1);：
因为时间问题，不对所有点进行似然计算，而是间隔选点，step就是步长.

lser_rang_count可以用rostopic echo --noarr /scan得到,max_beams由自己设置. 如果step小于1,程序里会设置为1.

因为寻找最近障碍物是最耗时的，可以将似然域预先计算好，搜索时直接查表即可。我理解的查表是查激光点在查栅格地图的坐标。

似然域模型的最后重新确定了每个粒子的重要性权重sample->weight

pf_update_sensor 函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
// 根据观测值更新滤波器
void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data)
{
  int i;
  pf_sample_set_t *set;
  pf_sample_t *sample;
  double total;

  set = pf->sets + pf->current_set;

  // 粒子总权重
  total = (*sensor_fn) (sensor_data, set);
  
  if (total > 0.0)
  {
		// Normalize weights
		double w_avg=0.0;
		for (i = 0; i < set->sample_count; i++)
		{
		  sample = set->samples + i;
		  w_avg += sample->weight;
		  sample->weight /= total;
		}
		// Update running averages of likelihood of samples
		w_avg /= set->sample_count;
		if(pf->w_slow == 0.0)
		  pf->w_slow = w_avg;
		else
		  pf->w_slow += pf->alpha_slow * (w_avg - pf->w_slow);
    
		if(pf->w_fast == 0.0)
		  pf->w_fast = w_avg;
		else
		  pf->w_fast += pf->alpha_fast * (w_avg - pf->w_fast);
  }
  else
  {
    // Handle zero total
    for (i = 0; i < set->sample_count; i++)
    {
      sample = set->samples + i;
      sample->weight = 1.0 / set->sample_count;
    }
  }
  return;
}

代码比较简单，就是196页的AMCL算法的7~11行。 不解的地方是 sample->weight最后又除以了粒子总权重

robot_pose_ekf源码分析|激光SLAM多传感器融合

robot_pose_ekf基本流程：

  1. 从每个传感器获取数据

  2. 检查它们是否有效，如果它们有效，则将它们相对于参考基准坐标系进行转换

  3. 当获得传感器信息时，它将被存储，直到所有传感器的信息可用为止。收到的每个数据都有自己的时间戳

  4. 一旦所有数据都可用，则在所有传感器数据均可用时，针对每个可用传感器数据更新扩展卡尔曼滤波器（在Orocos-BFL库中定义）。即：如果来自里程计的数据在时间t_0（> 0）可用，则来自imu的数据在时间t_1（> t_0）处获得，而来自视觉里程计的数据在时间t_2（> t_1）处获得，则在时间t_1对所有三组数据进行滤波。

  5. 该融合的传感器数据被转换为里程计消息，并在话题/odom_combined上发布

Matlab实现高斯分布|Matlab

一般是两种方法

1
2
3
4
5
x=[-20:0.01:20]

%y2 = pdf('Normal',x,1,5)
y = gaussmf(x,[1 5]);
plot(x,y2)

运行结果


深蓝色区域是距平均值小于一个标准差之内的数值范围。在正态分布中，此范围所占比率为全部数值之68%，根据正态分布，两个标准差之内的比率合起来为95%；三个标准差之内的比率合起来为99%。