皆さん、こんにちは !!
「一つずつ丁寧に」がmottoのjnです !!
自己位置推定について説明します。
前回と同様にAMCLを理解していきます。
Probabilistic Roboticsのmeasurement modelはいくつか定義されていますがlikelihood field range finder modelの説明をします。
AMCL 1 (measurement modelのみの説明)
下記を理解します。
src/amcl_node.cpp
/scan topicを受信したらlaserReceived()関数が呼び出されます。
measurement modelのみの説明なので最初にupdateFilter()関数を読みます。
必要な準備をした後にsensorUpdate()関数を呼び出しています。この関数は準備が多いのでsensorUpdate()関数の方が重要ですがparameterの理解が深まるので読み飛ばさない方が良いです。
updateFilter()関数の理解のために下記の様にfprintfを埋め込みます。
fprintf( stderr, "laser_index: %d\n", laser_index );
fprintf( stderr, "laser_scan->ranges.size(): %d\n", ldata.range_count );
fprintf( stderr, "range_max: %f\n", ldata.range_max );
fprintf( stderr, "range_min: %f\n", range_min );
やはり手を動かすと理解が深まります。上記を適切な箇所に書き入れてみましょう。
amcl_node.cpp(updateFilter()関数を抜粋。fprintf埋め込み済)
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/* Author: Brian Gerkey */
bool AmclNode::updateFilter(
const int & laser_index,
const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
const pf_vector_t & pose)
{
nav2_amcl::LaserData ldata;
ldata.laser = lasers_[laser_index];
ldata.range_count = laser_scan->ranges.size();
fprintf( stderr, "laser_index: %d\n", laser_index );
fprintf( stderr, "laser_scan->ranges.size(): %d\n", ldata.range_count );
// To account for lasers that are mounted upside-down, we determine the
// min, max, and increment angles of the laser in the base frame.
//
// Construct min and max angles of laser, in the base_link frame.
// Here we set the roll pich yaw of the lasers. We assume roll and pich are zero.
geometry_msgs::msg::QuaternionStamped min_q, inc_q;
min_q.header.stamp = laser_scan->header.stamp;
min_q.header.frame_id = nav2_util::strip_leading_slash(laser_scan->header.frame_id);
min_q.quaternion = orientationAroundZAxis(laser_scan->angle_min);
inc_q.header = min_q.header;
inc_q.quaternion = orientationAroundZAxis(laser_scan->angle_min + laser_scan->angle_increment);
try {
tf_buffer_->transform(min_q, min_q, base_frame_id_);
tf_buffer_->transform(inc_q, inc_q, base_frame_id_);
} catch (tf2::TransformException & e) {
RCLCPP_WARN(
get_logger(), "Unable to transform min/max laser angles into base frame: %s",
e.what());
return false;
}
double angle_min = tf2::getYaw(min_q.quaternion);
double angle_increment = tf2::getYaw(inc_q.quaternion) - angle_min;
// wrapping angle to [-pi .. pi]
angle_increment = fmod(angle_increment + 5 * M_PI, 2 * M_PI) - M_PI;
RCLCPP_DEBUG(
get_logger(), "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
if (laser_max_range_ > 0.0) {
ldata.range_max = std::min(laser_scan->range_max, static_cast<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, static_cast<float>(laser_min_range_));
} else {
range_min = laser_scan->range_min;
}
fprintf( stderr, "range_max: %f\n", ldata.range_max );
fprintf( stderr, "range_min: %f\n", range_min );
// The LaserData destructor will free this memory
ldata.ranges = new double[ldata.range_count][2];
for (int i = 0; i < ldata.range_count; i++) {
// amcl doesn't (yet) have a concept of min range. So we'll map short
// readings to max range.
if (laser_scan->ranges[i] <= range_min) {
ldata.ranges[i][0] = ldata.range_max;
} else {
ldata.ranges[i][0] = laser_scan->ranges[i];
}
// Compute bearing
ldata.ranges[i][1] = angle_min +
(i * angle_increment);
}
lasers_[laser_index]->sensorUpdate(pf_, reinterpret_cast<nav2_amcl::LaserData *>(&ldata));
lasers_update_[laser_index] = false;
pf_odom_pose_ = pose;
return true;
}
LGPLだったのか。知らなかった。
LiDARのsensor値の個数
simulationのLiDAR modelのsamplesの設定を360にしてあるのでlaser_scan->ranges.size()は360です。
sensorのsample数のことです。sensor値を取り込んだ後の文脈のsampleはparticle filerを指します。ややこしい。
simulationのLiDAR modelのmax_angleを6.28(rad)で設定しているのでほぼ2 piなのでほぼ360(deg)の範囲の中で大体均等に360 sampleを取ってくれてると思います。
LiDARの最大距離
range_maxは、LiDARの最大距離の設定です。
障害物までのeuclidean distanceのようです。LiDARなので距離と角度を得るのでsqrtとか考えなくていいはずです。
正の値で設定した場合、nav2のyaml(nav2_bringup/params/nav2_params.yaml)で設定した値とmodel.sdfで設定した値を比較して小さい方が使われます。
実機であればLiDAR屋さんのros2 driverがpublishするtopicの中のrange_maxとの比較になるはずです。simlationでもmodel.sdfで設定した値をlidar modelがpublishしてます。
ゼロまたは負の値で設定した場合、topicの中の値が使われます。
後述するsensorFunction()関数の中で判定する処理をしています。
LiDARの最小距離
range_minは、LiDARの最小距離の設定です。
正の値で設定した場合、nav2のyaml(nav2_bringup/params/nav2_params.yaml)で設定した値とmodel.sdfで設定した値を比較して大きい方が使われます。
実機であればLiDAR屋さんのros2 driverがpublishするtopicの中のrange_minとの比較になるはずです。simlationでもmodel.sdfで設定した値をlidar modelがpublishしてます。
ゼロまたは負の値で設定した場合、topicの中の値が使われます。
後述するsensorFunction()関数の中で判定する処理をしています。
scan
LiDARが1回分のscanを実行してからpublishするという暗黙の了解があると分かります。正確にはLiDARのros2 driverですね。
360のsampleのうち200だけpublishして、後から残りの160をpublishすることもあるのかなと想像していたのですが、そんな面倒なことはしないようです。
simpleに1周分のscanを貰って自己位置を更新してます。
これで十分に自律走行できているのでsimpleなまま考えることにします。
このためupdateFilter()の前にgetOdomPose()関数でscanをsubscribeしたときの姿勢をtf_bufferから探すと読み取れる処理をしているのですが、得られる姿勢は1つです。
こちらも最もsimpleです。
AMCL 2 (measurement modelのみの説明)
sensorUpdate()関数を読みます。
likelihood_field_model.cpp
下記のどちらかを読みます。
src/sensors/laser/likelihood_field_model.cpp
src/sensors/laser/likelihood_field_model_prob.cpp
beam_model.cppは無視します。
障害物に対する距離と角度または位置が得られるsensorの機能を抽出して尤度場で表現したもの、を使います。
下記にlikelihood_field_model_prob.cppはbeam skipができると書いてあります。
beam skipはcost mapに無い障害物を無視できる機能です。
ここではlikelihood_field_model.cppを読むことにします。
sensorUpdate()関数はmax_beams_の値が小さすぎないかを検査してからpf_update_sensor()関数を呼び出しているだけです。
max_beams
max_beams_はnav2のyaml(nav2_bringup/params/nav2_params.yaml)で設定した値です。
ROS1の頃で初期設定が30で、ROS2で初期設定が60です。
1回の更新で何点使うかの設定のようです。
処理負荷的に破綻しない範囲内で増やした方が精度は良くなるはずです。
1以下の設定だったらparticle filterの更新をやらない実装になってます。
不要の人はこの検査は削除でいいと思います。
AMCL 3 (measurement modelのみの説明)
sensorFunction()関数を読みます。
pf_update_sensor()関数の中で実行される関数でこれが本体と言うべき処理をしています。
nav2_bringup/params/nav2_params.yamlで設定した値の下記が使われています。
- z_hit
- z_rand
- laser_likelihood_max_dist
- sigma_hit
- max_beams
尤度を計算してparticle filterの重みの更新をやっています。
sensorFunction()関数の中でmax_beams_で割り算しているので、例えば1つの/scanの中に360個のsensor値があり、それが1度ずつの値として、max_beams_が60とすると6度ずつのsensor値となると思います。
割り算するか、randomでも似た様なことになるよな、とか考えていたのですが、これも最もsimpleな設計(=割り算)でした。
下記が末尾にある行を追加または変更しています。
likelihood field modelの理解のためにOpenCVを使って状況がわかるような表示を追加しています。
- // add
- // modify
OpenCVをinstall。
$ sudo apt install libopencv-dev
CPATHを追記します。
cmakeが詳しい人はCMakeLists.txtで対応してください。というか管理という観点ではCMakeLists.txtで対応した方が少し良いです。
.bashrc
PATH=/usr/pkg/bin:$PATH
PATH=/home/jn/.local/bin:$PATH
export PATH
CPATH=/usr/include/opencv4/:$CPATH
export CPATH
忘れずに。
$ . ~/.bashrc
linkはCMakeLists.txtで対応します。
CMakeLists.txt
86 target_link_libraries(${library_name}
87 map_lib pf_lib sensors_lib opencv_core opencv_highgui opencv_imgproc opencv_imgcodecs
88 )
準備は完了。source codeを読んでいきましょう。
likelihood_field_model.cpp
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#include <math.h>
#include <assert.h>
#include <opencv2/opencv.hpp> // add
#include "nav2_amcl/sensors/laser/laser.hpp"
namespace nav2_amcl
{
char str[20]; // add
int file_index = 0; // add
int counter = 0; // add
cv::Mat img( 768, 768, CV_8UC3 ); // add
LikelihoodFieldModel::LikelihoodFieldModel(
double z_hit, double z_rand, double sigma_hit,
double max_occ_dist, size_t max_beams, map_t * map)
: Laser(max_beams, map)
{
z_hit_ = z_hit;
z_rand_ = z_rand;
sigma_hit_ = sigma_hit;
map_update_cspace(map, max_occ_dist);
}
double
LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set)
{
LikelihoodFieldModel * 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;
fprintf( stderr, "%s\n", __func__ ); // add
self = reinterpret_cast<LikelihoodFieldModel *>(data->laser);
// Pre-compute a couple of things
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);
// Step size must be at least 1
if (step < 1) {
step = 1;
}
total_weight = 0.0;
img = cv::Mat::zeros(768, 768, CV_8UC3); // add
for ( int k=0; k < 384; ++k ) { // add
for ( int l=0; l < 384; ++l ) { // add
z = self->map_->cells[MAP_INDEX(self->map_, k, l)].occ_dist; // add
int b = (int)10/z; // add
if ( 255 < b ) { b = 255; } // add
int r = (int)8/z; // add
if ( 8 < r ) { r = 8; } // add
cv::circle( img, cv::Point(k*2,l*2), r, cv::Scalar(b,0,0), 1 ); // add
} // add
} // add
fprintf( stderr, "sample numbers: %d\n", set->sample_count ); // add
// Compute the sample weights
for (j = 0; j < set->sample_count; j++) {
sample = set->samples + j;
pose = sample->pose;
// Take account of the laser pose relative to the robot
pose = pf_vector_coord_add(self->laser_pose_, pose);
p = 1.0;
for (i = 0; i < data->range_count; i += step) {
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
// This model ignores max range readings
if (obs_range >= data->range_max) {
continue;
}
// Check for NaN
if (obs_range != obs_range) {
continue;
}
// 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);
// Convert to map grid coords.
int mi, mj;
mi = MAP_GXWX(self->map_, hit.v[0]);
mj = MAP_GYWY(self->map_, hit.v[1]);
int x, y; // add
double w; // add
x = MAP_GXWX(self->map_, pose.v[0]); // add
y = MAP_GYWY(self->map_, pose.v[1]); // add
// Part 1: Get distance from the hit to closest obstacle.
// Off-map penalized as max distance
if (!MAP_VALID(self->map_, mi, mj)) {
z = self->map_->max_occ_dist;
} else {
z = self->map_->cells[MAP_INDEX(self->map_, mi, mj)].occ_dist;
}
// Gaussian model
// NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
pz = self->z_hit_ * exp(-(z * z) / z_hit_denom); // modify
// Part 2: random measurements
pz += self->z_rand_ * z_rand_mult;
// fprintf( stderr, "map x: %d, map y: %d,dist: %f\n", mi, mj, z );
// 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
// works well, though...
p += pz * pz * pz;
if ( 100 == counter ) // add
{ // add
cv::circle( img, cv::Point(mi*2,mj*2), 8*pz, cv::Scalar(0,255*pz,0), 1 ); // add
w = sample->weight * p; // add
// fprintf( stderr, "sample weight: %f\n", w ); // add
cv::circle( img, cv::Point(x*2,y*2), 1600*w, cv::Scalar(0,0,30000*w), 1 ); // add
// for ( int k=0; k < 384; k += 4 ) { // add
// for ( int l=0; l < 384; l += 4 ) { // add
// if (!MAP_VALID(self->map_, k, l)) { // add
// z = self->map_->max_occ_dist; // add
// } else { // add
// z = self->map_->cells[MAP_INDEX(self->map_, k, l)].occ_dist; // add
// } // add
// int b = (int)100/z; // add
// if ( 255 < b ) { b = 255; } // add
// int r = (int)8/z; // add
// if ( 8 < r ) { r = 8; } // add
// cv::circle( img_1, cv::Point(k*2,l*2), r, cv::Scalar(b,0,0), 1 ); // add
// } // add
// } // add
} // add
}
if ( 100 < counter ) // add
{ // add
sprintf( str, "%04d.bmp", file_index); // add
cv::imwrite( str, img ); // add
++file_index; // add
counter = 0; // add
} // add
++counter; // add
sample->weight *= p;
total_weight += sample->weight;
}
return total_weight;
}
bool
LikelihoodFieldModel::sensorUpdate(pf_t * pf, LaserData * data)
{
if (max_beams_ < 2) {
return false;
}
pf_update_sensor(pf, (pf_sensor_model_fn_t) sensorFunction, data);
return true;
}
} // namespace nav2_amcl
LGPLか。
上記の理解の促進のため動画で確認します。
1つ目の動画。走行の様子。下から上に走行してます。
2つ目の動画。こちらは90度時計回りになってます。さらに横軸を中心に180度回転しているようです。
上側の左から右に走行してます。
mapの各点のmapの中の最も近い黒い部分までの距離occ_distを青で描画しています。
particle filterの位置を赤で描画しています。
LiDARのhitの様子を緑で描画しています。全てrobotから見て奥側に出てますね。robotとLiDARの位置関係のoffsetの値が間違っているのかな ?
hitはLiDARのsensor値をそれぞれのparticle filterから見た場合の値になります。
この動画でAMCLのmeasument modelの見える化は大体満たしていると考えています。
initial poseをsetするまではおかしなところをhitとなってます。
hitしたところがcost mapの黒い部分になっている割合が高く、緑が大きいのが多いときは赤が大きくなります。
2つくらい大きな障害物を置いたら自己位置推定は滅茶苦茶になりそうですね。
particle filter
有望と思われるparticle filterとresamplingで消されそうな雑魚particle filterがいる様子。緑は消しています。
拡大するとocc_distの値の様子がわかります。
小さいparticle filterと対応するhitは壁から離れた小さい緑になります。
大きいparticle filterと対応するhitはほぼ壁は一致して大きい緑になります。
resampling
resamplingで選択と集中を完了したと思われる様子。見込みの無い雑魚を外した後。再構築。緑は消しています。
上記の画像はMacBookで見ると綺麗です。
source codes
以下は正しいかは確かめられてないのでちょっとした参考程度に読んでください。
step = (data->range_count - 1) / (self->max_beams_ - 1);
sensorの値の数を1回の更新で何個使うかがstepに入ります。
// Compute the sample weights
for (j = 0; j < set->sample_count; j++) {
sample = set->samples + j;
pose = sample->pose;
set->sample_countにはparticle filterの数が入っています。重みを計算するという記述からも予想できます。
poseはparticle filterの位置と向きです。
forが二重になっていて外側のfor loopでは1つずつsample(=particle filter)を取り出しています。
内側のfor loopではscanをmax_beamで設定した個数でsample(=particle filter)の重みを更新しています。ここでは360のsensor値の中の60個を使う設定になってます。360個の中の360個を使う設定としても動作すると思います。
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
sensor値から距離(=range)と角度(=bearing)を取り出します。
if (obs_range >= data->range_max) {
continue;
}
遠すぎるsensor値は無視する。または、ある程度近くでも、設定より遠いsensor値は無視する。
// 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);
landmarkの位置。world座標系。但しrobotの姿勢ではなくparticle filter 1個の姿勢。
particle filterごとにposeの値が異なるためhitへの代入値が変わりmi,mjが変わります。
ばら撒いたparticle filterの位置がおかしいとzが大きくなります。zが大きいとpzが小さくなります。やがて消えるでしょう。(障害物があったら、もっとおかしくなります。)
だからscan matchingはしてないけど、それに近いことをしていることになるのかな。
障害物が無いこととcost mapが正確なことも要求していると思われます。
これ。
landmark
// Convert to map grid coords.
int mi, mj;
mi = MAP_GXWX(self->map_, hit.v[0]);
mj = MAP_GYWY(self->map_, hit.v[1]);
5cmずつとかのgridのmapは作成済みとして1pixelにlandmarkの位置を対応させます。
自分が使っているLiDARのsensor値は1.23456789(m)みたいにmmより小さい値まで入っています。
// Part 1: Get distance from the hit to closest obstacle.
z = self->map_->cells[MAP_INDEX(self->map_, mi, mj)].occ_dist;
該当の1pixel(mi,mj)から最も近い障害物(=cost mapの中の黒い部分)までの距離をzに入れます。
occ_distの値はLikelihoodFieldModelのconstructorで呼ばれるmap_cspace.cppのmap_update_cspace()関数で入れられます。constructorなので一度だけです。
障害物はLiDARから見える障害物でなくてcost mapの中の黒い部分のことです。
というわけでparticle filterとhitとの距離は捨ててます。
cost mapの黒い部分、それぞれ1pixelになるのでoccupyと書かれる部分、これの情報を頼りにparticle filterの重みを更新しています。
cost mapと現実が一致しているとして、障害物と遠い場合はLiDARから得られるhitの数が少なくzにほぼ1が入っているとしても重みはそこまで上がらない、と。sensor値が100万個とかあっても同じ議論でいけるはず。
障害物と近い場合はLiDARから得られるhitの数が多くなりzにほぼ1が入っていたら重みは上がる、と。
つまり、遠いときは精度の低い予測となり、近いときは精度の高い予測となる、と。
思い切った設計だな。
地図に無い障害物のhitから得られるzは大きい値となりますね。AMCLはhitの最も近い1pixelからcost mapにある黒い部分との距離として計算しますね。
やっぱり地図に無い障害物は無視しないといけないな。で、beam skipが必要になる、と。
地図の自動更新とかやろうと思ったらparticle filterとhitとの距離は必要そうだな。
提案されているだろうから、論文を探して読むか。
// Gaussian model
// NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
pz = self->z_hit_ * exp(-(z * z) / z_hit_denom);
観測誤差の設計です。f(x)のx=zです。
z_hit_denomのdenomは「分母」のことです。
正規分布。\[f(x)=\frac{1}{\sqrt{2\pi}\sigma}e^{-\tfrac{(x-\mu)^2}{2\sigma^2}}\]
正規化定数を無視しているのだと思いますが、「あるべき姿は1/(sqrt(2pi)*sigma)だ」とcommentに書いてある。どっちやねん。
正規分布と正規化定数の説明はこちら。
// Part 2: random measurements
pz += self->z_rand_ * z_rand_mult;
こちらも観測誤差の設計です。
z_hit_denomもそうですがz_rand_multをforの外で計算してあります。明らかに無駄なことはしない、と。
z_rand_とz_hit_は合計1の制約があるはずです。
z_rand_multはLiDARの最大距離で1を割った値です。
sample->weight *= p;
自分の重みを更新しています。
p *= pz;とはしていない値がpに入っています。beam skipのときにp += pz * pz * pz;の方が上手くいくというようなことが書かれています。
total_weight += sample->weight;
全体の重みを更新しています。
値が大きいと自己位置推定が正しい可能性が高く、値が小さいと自己位置推定が間違っている可能性が高いことになります。
AMCL 4 (measurement modelのみの説明)
pf_update_sensor()関数を読みます。
sensorFunction()関数の処理を抜けた後に重みの正規化をやっています。
その後でnav2_bringup/params/nav2_params.yamlで設定した値の下記が使われています。
これがAMCLのAです。adaptiveです。
- recovery_alpha_fast
- recovery_alpha_slow
(pf->alpha_fast =) recovery_alpha_fast
Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.1.
(pf->alpha_slow =) recovery_alpha_slow
Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.001.
good valueと書いてあるのに両方とも初期設定は0.0です。なんでや。
recovery_alpha_fastが0.1は大きいと思います。
一旦MCLのみとします。
sensor_fnがsensorFunction()関数です。
pf.c
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;
// Compute the sample weights
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;
}
} else {
// Handle zero total
for (i = 0; i < set->sample_count; i++) {
sample = set->samples + i;
sample->weight = 1.0 / set->sample_count;
}
}
}
for (i = 0; i < set->sample_count; i++) {
sample = set->samples + i;
w_avg += sample->weight;
sample->weight /= total;
}
particle filterを1つずつtotalで割っています。weightの合計を1にしています。
resamplingの中でも同じことをしています。recovery_alpha_fastとrecovery_alpha_slowをゼロに設定している場合はcomment outしても今まで通りに走行します。
こちらが全部。
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;
// Compute the sample weights
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 (Prob Rob p258)
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;
}
}
}
まとめ
上記の通りmotion modelよりは複雑ですがそこまで難しくはないです。
広告
IT開発関連書とビジネス書が豊富な翔泳社の通販『SEshop』
さくらのレンタルサーバ
ムームードメイン
Ｏｉｓｉｘ（おいしっくす）
らでぃっしゅぼーや
珈琲きゃろっと
エプソムソルト
