サンプルデモ

f:id:sy4310:20210919221829g:plain

参考書籍

詳解　確率ロボティクス　Ｐｙｔｈｏｎによる基礎アルゴリズムの実装 (ＫＳ理工学専門書)

作者:上田隆一
講談社

Amazon

センサ値における誤差の分類

偶然誤差
系統誤差
過失誤差
過失誤差は、実験中にデータの記録を間違えるような事象を指す

誤差を生む5種類の事象

雑音: ガウス分布状にセンサ値をばらつかせる
バイアス: 常に距離、方角に一定値を加えす
ファントム: 見えないはずのランドマークを観測する
見落とし: 見えるはずのランドマークを見落とす
オクルージョン: ランドマークが他の物体に隠れて見えなくなる

不確かさの実装

雑音

距離 $l$ の計測は遠くなるほど曖昧になる
方角 $\varphi$ はあまり曖昧にならないとする
距離に比例する標準偏差でガウス分布に従う雑音を付加する
方角に対しては一定の標準偏差でガウス分布に従う雑音を付加する

# 不確かさの実装で追加する部分のみ記載

using Plots, Random, Distributions # 乱数と分布を扱うパッケージの追加
pyplot()

mutable struct Sensor
  # メンバ変数を追加
  dist_noise_rate # 距離に加える雑音の標準偏差の割合
  dir_noise # 方角に加える雑音の標準偏差

  # 初期化時の入力引数を追加
  function Sensor(
                  dist_noise_rate=0.0, # デフォルト値は0
                  dir_noise=0.0) # デフォルト値は0
    # 引数で値を指定しメンバ変数にセット
    self.dist_noise_rate = dist_noise_rate
    self.dir_noise = dir_noise
    return self
  end
end

# 雑音を付加する関数
# 引数 self::Sensor センサクラスのオブジェクト
# 引数 obsrv::Array 観測値 [distance, direction]
function noise(self::Sensor, obsrv::Array)
  ell = rand(Normal(obsrv[1], obsrv[1] * self.dist_noise_rate)) # 距離に比例した標準偏差を持つガウス分布に従う乱数
  phi = rand(Normal(obsrv[2], self.dir_noise)) # 方角は一定の標準偏差を持つガウス分布に従う乱数
  return [ell, phi] # 雑音を付加した距離と方角を返す
end

# 観測値を計算してセットする関数
# 雑音を付加する関数を追加
# 引数 self::Sensor センサクラスのオブジェクト
# 引数 sns_pose::Array センサの姿勢 [x, y, theta]
function data(self::Sensor, sns_pose::Array)
  observed = []
  for obj in self.map.objects
    obsrv = observation_function(sns_pose, obj.pose)
    if visible(self, obsrv)
      obsrv = noise(self, obsrv) # 雑音を付加
      push!(observed, (obsrv, obj.id))
    end
  end
  self.last_data = observed
end

サンプルデモ
github.com

バイアス

距離 $l$ を一定の割合、方角 $\varphi$ を一定の値だけ恒久的にずらす
雑音と同様にガウス分布からドローして決定する

# 不確かさの実装で追加する部分のみ記載

using Plots, Random, Distributions # 乱数と分布を扱うパッケージの追加
pyplot()

mutable struct Sensor
  # メンバ変数を追加
  dist_bias_rate_std # 距離に加えるバイアスの標準偏差の割合
  dir_bias # 方角に加えるバイアスの標準偏差

  # 初期化時の入力引数を追加
  function Sensor(
                  dist_bias_rate_stddev=0.0, # デフォルト値は0
                  dir_bias_stddev=0.0) # デフォルト値は0
    # 平均0, 引数で与えた標準偏差を持つガウス分布からドローして設定
    self.dist_bias_rate_std = rand(Normal(0.0, dist_bias_rate_stddev)) # 距離のバイアス
    self.dir_bias = rand(Normal(0.0, dir_bias_stddev)) # 方角のバイアス
    return self
  end
end

# バイアスを付加する関数
# 引数 self::Sensor センサクラスのオブジェクト
# 引数 obsrv::Array 観測値 [distance, direction]
function bias(self::Sensor, obsrv::Array)
  # 距離と方角にそれぞれバイアスを加えたものを返す
  return obsrv + [obsrv[1] * self.dist_bias_rate_std, self.dir_bias]
end

# 観測データを計算してセットする関数
# バイアスを付加する関数を追加
# 引数 self::Sensor センサクラスのオブジェクト
# 引数 sns_pose::Array センサの姿勢 [x, y, theta]
function data(self::Sensor, sns_pose::Array)
  observed = []
  for obj in self.map.objects
    obsrv = observation_function(sns_pose, obj.pose)
    if visible(self, obsrv)
      obsrv = bias(self, obsrv) # バイアスを付加
      obsrv = noise(self, obsrv) # 雑音を付加
      push!(observed, (obsrv, obj.id))
    end
  end
  self.last_data = observed
end

サンプルデモ
github.com

ファントム

存在しないはずのランドマークを観測してしまうケース

# 不確かさの実装で追加する部分のみ記載

using Plots, Random, Distributions # 乱数と分布を扱うパッケージの追加
pyplot()

mutable struct Sensor
  # メンバ変数を追加
  phantom_dist_x # ファントムのX座標
  phantom_dist_y # ファントムのY座標
  phantom_prob # ファントムが出現する確率

  # 初期化時の入力引数を追加
  function Sensor(
                  phantom_prob=0.0, # デフォルト値は0
                  phantom_rng_x::Tuple=(-5.0, 5.0), # X方向のファントムの出現範囲
                  phantom_rng_y::Tuple=(-5.0, 5.0)) # Y方向のファントムの出現範囲
    rx = phantom_rng_x # 出現範囲をセット
    ry = phantom_rng_y
    self.phantom_dist_x = Uniform(rx[1], rx[2]) # ファントム出現位置をドローする一様分布オブジェクト
    self.phantom_dist_y = Uniform(ry[1], ry[2])
    self.phantom_prob = phantom_prob # ファントムの出現確率
    return self
  end
end

# ファントムを出現させる関数
# 引数 self::Sensor センサクラスのオブジェクト
# 引数 sns_pose::Array センサの姿勢 [x, y, theta]
# 引数 obsrv::Array 観測値 [distance, direction]
function phantom(self::Sensor, sns_pose::Array, obsrv::Array)
  # 0~1.0の乱数を出力
  # ファントムの出現確率がそれを上回ったら位置を計算
  if rand(Uniform()) < self.phantom_prob
    phantom_pose = [rand(self.phantom_dist_x), rand(self.phantom_dist_y)] # 出現位置
    return observation_function(sns_pose, phantom_pose) # 観測方程式から距離と方角を計算
  else
      return obsrv # そうでなければ元の観測値をそのまま返す
  end
end

# 観測データを計算してセットする関数
# ファントムを出現させる関数を追加
# 引数 self::Sensor センサクラスのオブジェクト
# 引数 sns_pose::Array センサの姿勢 [x, y, theta]
function data(self::Sensor, sns_pose::Array)
  observed = []
  for obj in self.map.objects
    obsrv = observation_function(sns_pose, obj.pose)
    obsrv = phantom(self, sns_pose, obsrv) # ファントム出現
    if visible(self, obsrv)
      obsrv = bias(self, obsrv) # バイアスを付加
      obsrv = noise(self, obsrv) # 雑音を付加
      push!(observed, (obsrv, obj.id))
    end
  end
  self.last_data = observed
end

サンプルデモ
github.com

見落とし

# 不確かさの実装で追加する部分のみ記載

using Plots, Random, Distributions # 乱数と分布を扱うパッケージの追加
pyplot()

mutable struct Sensor
  # メンバ変数を追加
  oversight_prob # 見落としが発生する確率

  # 初期化時の入力引数を追加
  function Sensor(oversight_prob=0.0) # デフォルト値は0
    self.oversight_prob = oversight_prob # 発生確率を引数からセット
    return self
  end
end

# 見落としを発生させる関数
# 引数 self::Sensor センサクラスのオブジェクト
# 引数 obsrv::Array 観測値 [distance, direction]
function oversight(self::Sensor, obsrv::Array)
  # 0~1.0の乱数を出力
  # 見落としの発生確率がそれを上回ったら何も返さない
  if rand(Uniform()) < self.oversight_prob
    return nothing
  else
    return obsrv # そうでなければ元の観測値をそのまま返す
  end
end

# 観測データを計算してセットする関数
# 見落としを発生させる関数を追加
# 引数 self::Sensor センサクラスのオブジェクト
# 引数 sns_pose::Array センサの姿勢 [x, y, theta]
function data(self::Sensor, sns_pose::Array)
  observed = []
  for obj in self.map.objects
    obsrv = observation_function(sns_pose, obj.pose)
    obsrv = phantom(self, sns_pose, obsrv) # ファントム出現
    obsrv = oversight(self, obsrv) # 見落とし発生
    if visible(self, obsrv)
      obsrv = bias(self, obsrv) # バイアスを付加
      obsrv = noise(self, obsrv) # 雑音を付加
      push!(observed, (obsrv, obj.id))
    end
  end
  self.last_data = observed
end

サンプルデモ
github.com

オクルージョン

センサで観測したい対象が別のものに隠される現象
「見落としていないけどセンサ値を大きく間違う」と定義する
センサと壁の間を通行者が遮り、計測した距離が実勢より近くなる

# 不確かさの実装で追加する部分のみ記載

using Plots, Random, Distributions # 乱数と分布を扱うパッケージの追加
pyplot()

mutable struct Sensor
  # メンバ変数を追加
  occlusion_prob # オクルージョンの発生確率

  # 初期化時の入力引数を追加
  function Sensor(occlusion_prob=0.0) # デフォルト値は0
    self.occlusion_prob = occlusion_prob # 発生確率を引数からセット
    return self
  end
end

# オクルージョンを発生させる関数
# 引数 self::Sensor センサクラスのオブジェクト
# 引数 obsrv::Array 観測値 [distance, direction]
function occlusion(self::Sensor, obsrv::Array)
  # 0~1.0の乱数を出力
  # 発生確率がこれを上回ったらオクルージョンを発生させる
  if rand(Uniform()) < self.occlusion_prob
    # 一定確率でセンサ値が真の値よりも小さくなる現象を実装
    # 最終的な距離が現在の値と観測可能な最大距離の間になるように一様分布で選ぶ
    ell = obsrv[1] - rand(Uniform()) * (self.dist_rng[2] - obsrv[1])
    phi = obsrv[2] # 方角はそのまま
    return [ell, phi]
  else
    return obsrv
  end
end

# 観測データを計算してセットする関数
# オクルージョンを発生させる関数を追加
# 引数 self::Sensor センサクラスのオブジェクト
# 引数 sns_pose::Array センサの姿勢 [x, y, theta]
function data(self::Sensor, sns_pose::Array)
  observed = []
  for obj in self.map.objects
    obsrv = observation_function(sns_pose, obj.pose)
    obsrv = phantom(self, sns_pose, obsrv) # ファントム出現
    obsrv = occlusion(self, obsrv) # オクルージョン発生
    obsrv = oversight(self, obsrv) # 見落とし発生
    if visible(self, obsrv)
      obsrv = bias(self, obsrv) # バイアスを付加
      obsrv = noise(self, obsrv) # 雑音を付加
      push!(observed, (obsrv, obj.id))
    end
  end
  self.last_data = observed
end