2017-05-02

Probabilistic Robotics - Chap.8 Mobile Robot Localization: Grid And Monte Carlo

確率ロボティクスロボット

8.1 Introduction

　８章では下記２つのローカライゼーションアルゴリズムについて述べる。

グリッドローカライゼーション

　beliefの計算にヒストグラムフィルタを使う。グリッドローカライゼーションは、グリッドを細かくすると、計算負荷が高くなり処理が遅くなるという問題がある。

モンテカルロローカライゼーション（MCL:Monte Carlo Localization）

　パーティクルフィルタを使ってローカライゼーションを行う、有名なアルゴリズム。欠点は色々とあるものの、誘拐ロボット問題や動的環境下への対応が可能。

これら２つのアルゴリズムは、下記の共通する特徴がある。

センサ生値を処理するため、特徴量の抽出といった加工処理が不要
ノンパラメトリック
グローバルローカライゼーションのための手法であり、誘拐ロボット問題に対応可能

　以降では、上記２つのアルゴリズムの詳細を述べる。

8.2 Grid Localization

8.2.1 Basic Algorithm

　グリッドローカライゼーションは、ヒストグラムフィルタを利用してグリッドマップ上でのロボットの事後beliefを近似する。つまり事後beliefは、下記の様に各グリッド ${\mathrm{x}_k}$ での存在確率 ${p_{k, t}}$ の集合
${ bel(x_t) = \{ p_{k, t} \} \tag{1} }$
として表せる。また各グリッド ${\mathrm{x_k}}$ の集合であるグリッドマップ ${X_t}$ は、
${ domain(X_t) \ = \ \mathrm{x}_{1, t} \ \cup \ \mathrm{x}_{2, t} \ \cup \ ... \ \cup \mathrm{x}_{k, t} \tag{2} }$
とできる。
　グリッドローカライゼーションの擬似コードを下記に示す。グリッドローカライゼーションは、入力として各グリッドでの存在確率 ${\{ p_{k, t} \}}$ と最新の観測結果、制御量、マップ情報を必要とする。そして処理結果として、最新の事後beliefを出力する。疑似コード中の ${\boldsymbol{motion\_model}}$ と ${\boldsymbol{measurement\_model}}$ は、Chapter5、6で紹介したどの方法を適用してもよい。

${ 1: \ \ \ \boldsymbol{Algorithm Grid\_localization}(\{p_{k, t}\}, u_t, z_t, m): \\ 2: \ \ \ \ \ for \ all \ k \ do \\ 3: \ \ \ \ \ \ \ \overline{p}_{k, t} = \sum_i p_{i, t-1} \boldsymbol{motion\_model}(mean(\mathrm{x}_k), u_t, mean(\mathrm{x}_i)) \\ 4: \ \ \ \ \ \ \ p_{k, t} = \eta \ \overline{p}_{k, t} \boldsymbol{measurement\_model}(z_t, mean(\mathrm{x}_k), m) \\ 5: \ \ \ \ \ endfor \\ 6: \ \ \ \ \ return \ \{p_{k, t}\} }$

8.2.2 Grid Resolution

　グリッドローカライゼーションにおける重要な変数は、グリッドの解像度である。グリッドの決め方には、トポロジカルな方法とメトリックな方法がある。トポロジカルな方法では、環境を特徴的な領域に分割する。ここで言う特徴的な領域とは、ドアや窓といったランドマークが存在する領域を指す。メトリックな方法では、環境の特性を考慮せずに均一なグリッドに分割する。分割数は、特徴的な領域は少ない場合が多いので、メトリックな方法の方が多い。
　分割数を多くすると計算負荷が高まるため、解像度は粗くなりがちである。粗さへの対応は、センサモデルと運動モデルの工夫が必要である。例えばメトリックな方法でグリッドを切った場合、グリッドが ${1m \times 1m}$ で、ロボットの速度が ${1m/s}$ とすると、運動モデルが頻繁に更新してもグリッドの遷移を検出できない。そのため、運動モデルの更新頻度を下げるといった対応が必要である。

8.2.3 Comuputational Consideration

　ローカライゼーション時の計算負荷を下げる方法は、沢山検討されている。

Model pre-caching

　予め特定の計測結果を保存しておき、現在の値とその値を比較することで、瞬時に状態を特定可能にする。

Sensor Subsampling

　センサデータを全て処理するのではなく、重要な一部のデータのみを処理する。

Delayed Motion Update

　制御や観測を行うよりも低い頻度でbeliefの更新を行う。

Selective Updating

　事後確率に対する閾値を設定しておき、閾値を超えているグリッドのみ更新を行う。

8.3 Monte Carlo Localization(MCL)

8.3.2 The MCL Algorithm

　ベーシックなMCLアルゴリズムでは、beliefはパーティクルのセットで表現される。beliefの更新方法は下記の擬似コードの通り。

${ \ 1: \ Algorighm \ MCL(\chi_{t-1}, u_t, z_t, m): \\ \ 2: \ \quad \overline{chi}_t = \chi_t = \varnothing \\ \ 3: \ \quad for \ m = 1 \ to \ M \ do \\ \ 4: \ \qquad x^{[m]}_t = \boldsymbol{sample\_motion\_model}(u_t, x^{[m]}_{t-1}) \\ \ 5: \ \qquad w^{[m]}_t = \boldsymbol{measurement\_model}(z_t, x^{[m]}_t, m) \\ \ 6: \ \qquad \overline{\chi}_t = \overline{\chi}_t + \langle x^{[m]}_t, w^{[m]}_t \rangle \\ \ 7: \ \quad endfor \\ \ 8: \ \quad for \ m=1 \ to \ M \ do \\ \ 9: \ \qquad draw \ i \ with \ probability \ \propto \ w^{[i]}_t \\ 10: \ \qquad add \ x^{[i]}_t \ to \ \chi_t \\ 11: \ \quad endfor \\ 12: \ \quad return \ \chi_t }$

初期beliefは、事前分布 ${p(x_0)}$ を元にランダムに生成した ${M}$ 個のパーティクルとして得る。この時各パーティクルにはimportance factor \ ${M^{-1}}$ を割り当てる。なお上記擬似コード中の ${\boldsymbol{sample\_motion\_model}}$ と ${\boldsymbol{measurement\_model}}$ は、グリッドローカライゼーションと同様に、５章、６章で紹介された方法を摘要できる。

8.3.4 Properties of MCL

　MCLは殆ど全ての実用上重要な分布を近似できる。一般的なパーティクル数 ${M}$ をセットするための戦略は、 ${u_t}$ と ${z_t}$ が更新されるまでリサンプリングしないことである。またMCLの利点として、ノンパラメトリックな近似ができる点がある。

8.3.5 Random Particle MCL: Recovery form Failures

　今までに述べたMCLのアルゴリズムでは、グローバルローカライゼーションは可能だが失敗時に復帰できない、誘拐ロボット問題には対処できない、といった問題がある。それは、パーティクルが一度現在の姿勢と異なる場所に集まると、正しい姿勢へ回復できないためである。
　これら問題へのシンプルな対応策として、ランダムパーティクルの注入する方法がある。これにより、運動モデルの中にランダムな部分空間が出来る。ではどうやってランダムにパーティクルを注入するべきか。ある種の推論を使って行うのがよい。
　１つの方法として、観測モデルを監視し、モデルと平均確率を関連付ける方法がある。（ToDo:よくわからん）平均値は、観測モデルを近似できる。通常数ステップにわたった平均を求めるのがよい。
　観測確率が低い理由は、ローカライゼーションの失敗以外にも、センサノイズが異常に高い、パーティクルが空間上に分散しすぎている（集まっていない）、などがある。
　パーティクスル数を決める時、数ステップの観測尤度の平均を維持し、より多くのステップの平均に関連付けるとよい。

　どの分布を使ってサンプリングするかについての２つ目の問題は、２つの方法で言える。姿勢空間全体を対象とした一様分布を元にパーティクスルをばらまき、現在の観測値を使ってそれらを重み付けする。

　例えばランドマーク検出モデルなどのセンサモデルでは、観測モデルの分布に沿ってパーティクルをばらまくことができる。この場合は尤度の沿ってパーティクルをばらまく。

　下記にランダムランプルのばらまきを考慮したMCLのアルゴリズムを述べる。
${ \ 1: \ Algorighm \ MCL(\chi_{t-1}, u_t, z_t, m): \\ \ 2: \ \quad static \ w_{slow}, w_{fast} \\ \ 3: \ \quad \overline{chi}_t = \chi_t = \varnothing \\ \ 4: \ \quad for \ m = 1 \ to \ M \ do \\ \ 5: \ \qquad x^{[m]}_t = \boldsymbol{sample\_motion\_model}(u_t, x^{[m]}_{t-1}) \\ \ 6: \ \qquad w^{[m]}_t = \boldsymbol{measurement\_model}(z_t, x^{[m]}_t, m) \\ \ 7: \ \qquad \overline{\chi}_t = \overline{\chi}_t + \langle x^{[m]}_t, w^{[m]}_t \rangle \\ \ 8: \ \qquad w_{avg} \ = \ w_{avg} + \frac{1}{M} w^{[m]}_t \\ \ 9: \ \quad endfor \\ 10: \ \quad w_{slow} = w_{slow} + \alpha_{slow}(w_{avg} - w_{slow}) \\ 11: \ \quad w_{fast} = w_{fast} + \alpha_{fast}(w_{avg} - w_{fast}) \\ 12: \ \quad for \ m=1 \ to \ M \ do \\ 13: \ \qquad with \ probability \ max\{0.0, \ 1.0 - w_{fast}/w_{slow} \} \ do \\ 14: \ \qquad \quad add \ random \ pose \ to \ \chi_t \\ 15: \ \qquad else \\ 16: \ \qquad \quad draw \ i \ \in \ \{1,...,N\} \ with \ probability \ \propto \ w^{[i]}_t \\ 17: \ \qquad \quad add \ x^{[i]}_t \ to \ \chi_t \\ 18: \ \qquad endwith \\ 19: \ \quad endfor \\ 20: \ \quad return \ \chi_t }$
このアルゴリズムは、短期・長期それぞれの平均尤度 ${p(z_t | z_{1:t-1}}, u_{1:t}, m$ をトラッキングする点で適応的である。短期・長期の平均尤度は１０、１１行目で求めている。なお上記 ${\alpha_{fast}}$ 、 ${\alpha_{slow}}$ は、短期・長期の平均を求める指数フィルタの減衰率であり、 ${0 \leq \alpha_{slow} \ll \alpha_{fast}}$ を満たすものとする。
　このアルゴリズムの肝であるランダムサンプルの追加は、１３行目で行っている。１３行目に示すように、短期の平均が長期の平均を下回る、つまり[tex:{1.0 - w_{fast}/w_{slow} \rt 0}}の時、ランダムサンプルが追加される。これにより、観測尤度が急に減少すると、その文ランダムサンプルの沢山追加される。

8.3.6 Modifying the Proposal Distribution

　MCLの提案分布は、MCLを非効率なものにする原因の１つである。もしも提案分布と目標分布の差が大きくなれば、精度向上のためにサンプル数を増やす必要がある。提案分布が非効率なことで、ノイズのないセンサの観測結果からロボットの姿勢が寸分の狂いもなくわかるとすると、MCLが失敗することになる。そのためMCLを使う場合、精度の良いセンサよりも悪いセンサを使うことが望ましい。別の方法として、観測モデルに人工的なノイズを乗せるようにてもよい。
　その他の方法として、サンプリングプロセスを改良も考えられる。例えば動作モデルと観測モデルの役割を入れ替える、混合モデルが考えられる。混合モデルでは、観測モデルがパーティクルの生成、重みの更新を行う。混合モデルは、誤差の低減や誘拐ロボット問題へ対応が可能だが、実装には困難が伴うという問題もある。

8.4 Localization in Dynamic Environment

　これまでに述べたローカライゼーション手法の重要な制約は、静的な環境を仮定している。そのため、人などの動体を考慮していない。ただし確率的なアプローチでは、ノイズを考慮しているため、結果としてある程度動体を考慮している。一方で、センサノイズは各ステップ毎に独立していると仮定しているが、実際には数ステップにまたがって影響している。
　こうした動的な環境に対応する方法として、状態拡張（state augmentation）と外れ値の除外（outlier rejection）という２つの基本技術がある。前者を適用することで、ロボットの姿勢だけでなく人の特性（位置、速度など）を推定できるフィルタを定義できる。ただし計算が複雑になると言った問題がある。後者は、センサデータがなぜこの値になったかの原因追求と、モデル化していない動体の影響を受けたと思われる高尤度なデータを排除する。
　この方法は、本質的にはEMアルゴリズムと同じことを行う。ここで、観測モデルとして6.3節の式(6.12)を考え、新たに変数 ${\overline{c}^k_t} \in \{ hit, short, max, rand \}$ を導入する。

${ p(z^k_t | x_t, m) = \begin{pmatrix} z_{hit} \\ z_{short} \\ z_{max} \\ z_{rand} \end{pmatrix}^T \cdot \begin{pmatrix} p_{hit} (z^k_t | x_t, m) \\ p_{short} (z^k_t | x_t, m) \\ p_{max} (z^k_t | x_t, m) \\ p_{rand} (z^k_t | x_t, m) \end{pmatrix} \tag{6.12) = (8.8} }$

ここで ${p_{short}}$ は、 ${ p(\overline{c}^k_t \ = \ short \ | \ z^k_t, z_{1:t-1}, u_{1:t}, m)}$ を示すものとする。

${ \ 1: \ Algorithm \ test\_range\_measurement(z^k_t, \hat{\chi}_t, m): \\ \ 2: \ \quad p \ = \ q \ = \ 0 \\ \ 3: \ \quad for \ m \ = \ 1 \ to \ M \ do \\ \ 4: \ \qquad p \ = \ p + z_{short} \cdot p_{short}(z^k_t | x^{[m]}_t, m) \\ \ 5: \ \qquad q \ = \ q + z_{hit} \cdot p_{hit}(z^k_t | x^{[m]}_t, m) + z_{short} \cdot p_{short}(z^k_t | x^{[m]}_t, m) + z_{max} \cdot p_{max}(z^k_t | x^{[m]}_t, m) + z_{rand} \cdot p_{rand}(z^k_t | x^{[m]}_t, m) \\ \ 6: \ endfor \\ \ 7: \ if \ p/q \ \leq \ \chi \ then \\ \ 8: \ \qquad return \ accept \\ \ 9: \ else \\ 10: \ \qquad return \ reject \\ 11: \ endif }$

　アルゴリズムは上記の通り。入力はbelief ${\overline{bel}(x_t)}$ の代替であるパーティクルの集合 ${\hat{\chi}_t}$ とレンジファインダの観測データ ${z_t}$ 、地図 ${m}$ である。出力は入力した ${\hat{\chi}_t}$ の"reject"か"accept"のどちらか一方である。
　reject/acceptの判断は、観測結果が想定よりも驚くほど短いかどうかで行う。地図上に人などの動体が存在する場合、壁よりも近い場所に何かを検出することになる。そのため、観測結果が想定よりも顕著に短い場合はrejectする。
　外れ値のrejectは、一般的には良い考え方である。測距計以外のセンサでも同様の考え方を使って観測モデルを更新できる。

2017-05-01

Probabilistic Robotics - Chap.5 Robot Motion

確率ロボティクスロボット

5.1 Introduction

　モーションモデルはベイズフィルタの予測ステップにおいて肝となる状態遷移確率を構成する。本章では、ロボットの運動はある平面上での移動のみを取り上げる。

5.2 Preliminaries

5.2.1 Kinematic Configuration

　ロボットの姿勢は下記の式で表せるものとする。
${ \begin{pmatrix} x \\ y \\ \theta \end{pmatrix} }$
ロボットの向きはよく、 ${bearing}$ や ${heading direction}$ と呼ばれる。また姿勢から向きを取ったものは ${位置}$ と呼ぶ。

5.2.2 Probabilistic Kinematics

　状態遷移確率は ${p(x_t | u_t, x_{t-1}}$ として表される。 ${x_t, x_{t-1}}$ はそれぞれ、時刻 ${t, t-1}$ でのロボット状態を、 ${u_t}$ は時刻 ${t}$ でのモーションコマンドを、それぞれ表すものとする。モーションモデルにおいて、 ${u_t}$ はオドメトリを指す場合もある。
　本章では、下記２種類の確率的モーションモデルを取り上げる。

速度モーションモデル

　 ${u_t = Velocity}$ なモデル。ただし速度を指令値通り制御することは、ノイズ等の都合上難しいことため、精度は良くないことが多い。

オドメトリモーションモデル

　 ${u_t = odometry}$ なモデル。速度モデルよりも精度が高いことが多い。
ただしオドメトリは動いた結果わかる情報である。そのため、動作の予測には使えない。そのため、速度モデルはモーションプランニングに、オドメトリモデルは状態推定に、それぞれ使うことが一般的である。以降ではそれぞれについて詳細を述べる。

5.3 Velocity Motion Model

　本モデルは、ロボットの速度 ${u_t}$ は並行運動 ${v_t}$ と回転運動 ${\omega_t}$ の２種類で構成されると過程する。なお ${v_t}$ と ${\omega_t}$ は、それぞれ１次元のスカラ値である。

5.3.1 Closed Form Calculation

　速度モデルは、下記のアルゴリズムで表現される。
${ 1: \ \ Algorithm motion\_model\_velocity(x_t, u_t, x_{t-1}):\\ 2: \ \ \ \ \ \mu = \frac{1}{2} \frac{(x - x') cos\theta + (y - y') sin \theta}{(y - y') cos\theta - (x - x') sin \theta}\\ 3: \ \ \ \ \ x^{*} = \frac{x + x'}{2} + \mu (y - y') \\ 4: \ \ \ \ \ y^{*} = \frac{y + y'}{2} + \mu (x' - x) \\ 5: \ \ \ \ \ r^{*} = \sqrt{(x - x^{*})^2 + (y - y^{*})^2} \\ 6: \ \ \ \ \ \Delta \theta = atan2(y' - y^{*}, x' - x^{*}) - atan2(y - y^{*}, x - x^{*}) \\ 7: \ \ \ \ \ \hat{v} = \frac{\Delta \theta}{\Delta t} r^{*} \\ 8: \ \ \ \ \ \hat{\omega} = \frac{\Delta \theta}{\Delta t} \\ 9: \ \ \ \ \ \hat{\gamma} = \frac{\theta' - \theta}{\Delta t} - \hat{\omega} \\ 10: \ \ \ \ return \ prob(v - \hat{v}, \alpha_1 v^2 + \alpha_2 \omega^2) \cdot prob(\omega - \hat{\omega}, \alpha_3 v^2 + \alpha_4 \omega^2) \cdot prob(\hat{\gamma}, \alpha_5 v^2 + \alpha_6 \omega^2) }$

ここで ${\alpha_1}$ から ${\alpha_6}$ は、ロボット固有の誤差パラメータでる。また ${prob(x, b^2)}$ は、モーションエラーをモデル化した関数を表す。この固有パラメータの決定方法について、特に本書には記載がないです。恐らく指令値と実測値の組み合わせを記録していき、その結果が良い感じに ${prob(x, b^2)}$ の結果とフィットするようにパラメータチューンが必要と思われます。

5.3.2 Sampling Algorithm

　5.3.1では計算で求めていましたが、本節ではパーティクルフィルタの様にサンプリングを介して解析に状態遷移確率を求める手法を紹介します。サンプル点から求める際の下記の手法を使う。
${ 1: \ \ Algorithm sample\_motion\_model\_velocity(u_t, x_{t-1}):\\ 2: \ \ \ \ \ \hat{v} = v + \boldsymbol{sample}(\alpha_1 v^2 + \alpha_2 \omega^2) \\ 3: \ \ \ \ \ \hat{\omega} = \omega + \boldsymbol{sample}(\alpha_3 v^2 + \alpha_4 \omega^2) \\ 4: \ \ \ \ \ \hat{\gamma} = \boldsymbol{sample}(\alpha_5 v^2 + \alpha_6 \omega^2) \\ 5: \ \ \ \ \ x^{'} = x - \frac{\hat{v}}{\omega} sin \theta + \frac{\hat{v}}{\omega} sin (\theta + \hat{\omega} \Delta t \\ 6: \ \ \ \ \ y^{'} = y + \frac{\hat{v}}{\omega} cos \theta - \frac{\hat{v}}{\omega} cos (\theta + \hat{\omega} \Delta t \\ 7: \ \ \ \ \ \theta^{'} = \theta + \hat{\omega} \Delta t + \hat{\gamma} \Delta t \\ 8: \ \ \ \ \ return \ x_t = (x^{'}, y^{'}, \theta^{'})^T\\ }$
２～４行目は、速度パラメータにノイズを加えます。その結果を使い、５～７行目で姿勢を計算しています。サンプリング手法を使う場合、運動方程式等の物理モデルを考慮せず、サンプル点の位置からのみ姿勢が求まるという利点がある。そのため実装の観点からすると、サンプリング手法を利用ことが楽である。

5.4 Odometry Motion Model

　オドメトリは、ドリフトやスリップによってエラーは生じるものの、速度に比べて計測誤差が小さい。また実際に動いた後でないと計測できないため、モーションプランニングには使えない。

5.4.1 Closed Form Calculation

　オドメトリをベースとしたモーションモデルは、今日の確率ロボティクスベースのシステムで中心的な役割を果たしている。
オドメトリは、ロボット内部の情報量である。そのため、実世界とリンクするためには座標変換が必要である。ただしドリフトやスリップが生じるため、変換行列は一定ではない。なおこの変換行列を求めることは、自己位置推定を行うことと等価である。
　オドメトリモデルは相対動作情報を使用する。相対動作情報とは、時刻 ${t-1}$ から ${t}$ の間で、どの程度動いたかを表す情報である。ロボットの時刻 ${t-1}$ と ${t}$ の時の姿勢のペアを ${u_t}$ とすると、
${ u_t = \begin{pmatrix} \overline{x}_{t-1} \\ \overline{x}_t \end{pmatrix} }$
となる。
　この動作情報から相対動作情報を取り出すと、回転・直進・回転の３ステップに分割できる。つまり ${( \begin{pmatrix} \delta_{rot1} \delta_{trans} \delta_{rot2} \end{pmatrix} }$
と表せる。この情報が分かれば、ある時刻 ${t-1}$ から ${t}$ の間の移動を再現できる。
確率的モーションモデルでは、上記３パラメータはノイズが加わっているものと仮定する。

以上を踏まえた、オドメトリモデルでの状態遷移確率を求めるアルゴリズムは下記の通りである。
${ 1: \ \ \ Algorithm \ motion\_model\_odometry(x_t, u_t, x_{t-1}):\\ 2: \ \ \ \ \ \delta_{rot1} = atan2(\overline{y}^{'} - \overline{y}, \overline{x}^{'} - \overline{x}) - \overline{\theta} \\ 3: \ \ \ \ \ \delta_{trans} = \sqrt{(\overline{x}^{'} - \overline{x})^2 + (\overline{y}^{'} - \overline{y})^2} \\ 4: \ \ \ \ \ \delta_{rot2} = \overline{\theta}^{'} - \overline{\theta} - \delta_{rot1} \\ 5: \ \ \ \ \ \hat{\delta}_{rot1} = atan2(y^{'} - y, x^{'} - x) - {\theta} \\ 6: \ \ \ \ \ \hat{\delta}_{trans} = \sqrt{(x^{'} - x)^2 + (y^{'} - y)^2} \\ 7: \ \ \ \ \ \hat{\delta}_{rot2} = \theta^{'} - \theta - \delta_{rot1} \\ 8: \ \ \ \ \ p_1 = \boldsymbol{prob}(\delta_{rot1} - \hat{\delta}_{rot1}, \alpha_1 \hat{\delta}^2_{rot1} + \alpha_2 \hat{\delta}^2_{trans}) \\ 9: \ \ \ \ \ p_2 = \boldsymbol{prob}(\delta_{trans} - \hat{\delta}_{trans}, \alpha_3 \hat{\delta}^2_{trans} + \alpha_4 \hat{\delta}^2_{rot1} + + \alpha_4 \hat{\delta}^2_{rot2}) \\ 10: \ \ \ \ \ p_3 = \boldsymbol{prob}(\delta_{rot2} - \hat{\delta}_{rot2}, \alpha_1 \hat{\delta}^2_{rot2} + \alpha_2 \hat{\delta}^2_{trans}) \\ 11: \ \ \ \ \ return \ p_1 \cdot p_2 \cdot p_3 }$
なお ${\boldsymbol{prob}(a, b^2)}$ は、平均 ${0}$ で分散 ${b^2}$ の誤差分布における ${a}$ の発生確率を返す関数とする。また、 ${\alpha_1}$ から ${\alpha_4}$ は、ロボットの動作を特定するロボット固有のパラメータである。

上記アルゴリズムにおいて、各行が行っている内容は下記の通り

２～４行目：オドメトリ情報から相対動作情報 ${\begin{pmatrix} \delta_{rot1} \delta_{trans} \delta_{rot2} \end{pmatrix} }$ を抽出。
５～７行目：入力した姿勢 ${x_{t-1}}$ と ${x_t}$ から理想とする相対動作情報を算出。
８～１０行目：相対動作情報の各要素に対するエラーを確率的に計算
１１行目：相対動作情報の各要素に対するエラー発生確率をかけ合わせ、目的姿勢である ${x_t}$ からのずれを返す。

5.4.2 Sampling Algorithm

　サンプルベースでオドメトリモデルを求めるアルゴリズムは下記の通りである。 ${\boldsymbol{sample}(a,b^2}$ は、 ${a}$ の発生確率を返す、平均 ${0}$ で分散 ${b^2}$ な確率分布関数である。

${ 1: \ \ \ Algorithm \ motion\_model\_odometry(x_t, u_t, x_{t-1}):\\ 2: \ \ \ \ \ \delta_{rot1} = atan2(\overline{y}^{'} - \overline{y}, \overline{x}^{'} - \overline{x}) - \overline{\theta} \\ 3: \ \ \ \ \ \delta_{trans} = \sqrt{(\overline{x}^{'} - \overline{x})^2 + (\overline{y}^{'} - \overline{y})^2} \\ 4: \ \ \ \ \ \delta_{rot2} = \overline{\theta}^{'} - \overline{\theta} - \delta_{rot1} \\ 5: \ \ \ \ \ \hat{\delta}_{rot1} = \delta_{rot1} - \boldsymbol{sample}(\alpha_1 \delta^2_{rot1} + \alpha_2 \delta^2_{trans}) \\ 6: \ \ \ \ \ \hat{\delta}_{trans} = \delta_{trans} - \boldsymbol{sample}(\alpha_3 \delta^2_{trans} + \alpha_4 \delta^2_{rot1} + \alpha_4 \delta^2_{rot2}) \\ 7: \ \ \ \ \ \hat{\delta}_{rot2} = \delta_{rot2} - \boldsymbol{sample}(\alpha_1 \delta^2_{rot2} + \alpha_2 \delta^2_{trans}) \\ 8: \ \ \ \ \ x^{'} = x + \hat{\delta}_{trans} cos(\theta + \hat{delta}_{rot1}) \\ 9: \ \ \ \ \ y^{'} = y + \hat{\delta}_{trans} sin(\theta + \hat{delta}_{rot1}) \\ 10: \ \ \ \ \ \theta^{'} = \theta + \hat{\delta}_{rot1} + \hat{\delta}_{rot2} \\ 11: \ \ \ \ \ return \ x_t = (x^{'}, y^{'}, \theta^{'})^T }$

5.5 Motion And Maps

　今まで扱っていた状態遷移確率では、周囲環境を考慮していなかった。そこで状態遷移確率のパラメータとして、新たに地図 ${m}$ を導入する。地図を知ることで、壁などの到達不可能な場所にいるのか、そうでない到達可能な場所にいるのかが判別できる。これにより、ロボットの姿勢をより精度良く求める事が可能となる。
　地図の導入により、状態遷移確率は ${p(x_t | u_t, x_{t-1}, m)}$ となる。地図を知ることによって、今まで議論してきた状態遷移確率では「遷移可能」としてきた場所に遷移出来なくなるので、
${ p(x_t | u_t, x_{t-1}) \neq p(x_t | u_t, x_{t-1}, m) }$
となる。よって新規なモデルとなることから、マップベースモーションモデルと呼ぶことにする。
　マープベースモデルを閉形式で計算することは難しい。一方で、都合の良いことに計算効率のよい近似方法がある。近似式は下記の通り。
${ p(x_t | u_t, x_{t-1}, m) = \eta \frac{p(x_t | u_t, x_{t-1}) p(x_t | m)}{p(x_t)} }$
ここで ${\eta}$ は正規化係数である。通常 ${p(x_t)}$ は一様分布で表される。なお周囲環境は動的に変化しないことから、 ${m}$ は定数として考える。もしも ${p(x_t | m) = 0}$ の時は、ロボットは地図上に存在する障害物上に存在することを意味する。
　なお上記近似には問題がある。モデルでは地図を考慮しているため、ロボットが存在できる姿勢は全て遷移対象となる。しかし、例えば壁の向こう側に一瞬で行くのは難しいが、そうした物理的に遷移が可能かは考慮していない。そのため、遷移先の姿勢への移動コスト加味するなどの工夫が必要である。

2017-04-30

マハラノビス距離

数学統計

統計学で用いられる、データ点が平均からどれだけ外れているかを判定するための手法。[2]によると、ユークリッド距離を標準偏差で割った値に該当する。

例えば2次元のデータを評価する時、主成分分析と共に用いる。第一主成分をX軸、第二主成分をY軸とした時、マハラノビス距離は主成分上での距離を指す。マハラノビス距離の算出式は、

${ \sqrt{(\vec{x} - \mu)^T \Sigma^{-1} (\vec{x} - \mu)} }$

である。つまり分散が大きいと距離が小さくなり、分散が小さくなると距離が大きくなる。これにより、外れ値の判定に用いられたりする。

参考までに、下記にマハラノビス距離を用いて乱数データの異常値検知をするサンプルコードを作成しました。

参考文献

[1]教師なし学習による異常値検知: マハラノビス距離 (理論編) - Qiita
[2]判別分析（マハラノビス）
[3]マハラノビス距離とは - Minitab
[4]Mahalanobis distance - Wikipedia

2017-04-30

Probabilistic Robotics - Chap.3 Gaussian Filters

確率ロボティクスロボット数学

3.2 The Kalman Filter
- 3.2.2 The Kalman Filter Algorithm
3.3 The Extended Kalman Filter
- 3.3.1 Why Liearlization?
- 3.3.2 Linealization Via Taylor Expansion
3.4 The Unscented Kalman Filter(UKF)
- 3.4.2 The UKF Alogorithm
- 3.5 The Information Filter（IF)
参考

3.2 The Kalman Filter

カルマンフィルタは線形ガウスシステムでフィルタリング（つまり余計なものを省くプロセス[1]）と予測を行うために開発された。カルマンフィルタは、連続状態空間でのbeliefを実装する。離散状態空間やハイブリッドな空間には対応しない。

時刻 ${t}$ でのbelief ${\ bel(x_t)}$ は、ベイズフィルタがマルコフ過程に加えて下記３点を満たす時、平均 ${\mu_t}$ と共分散 ${\sigma_t}$ のガウス分布で表せる。

【１点目】
状態遷移確率 ${p(x_t | u_t, x_{t-1})}$ が線形関数で表わせ、状態がガウスノイズを加味した下記の式で表せる。
${x_t = A_t x_{t-1} + B_t u_t + \epsilon_t \tag{1}}$
ここで、 ${x_t、x_{t-1}}$ は状態ベクトル、 ${u_t}$ は制御ベクトルを、それぞれ表す。また ${A_t}$ は ${n \times n}$ の正方行列で、 ${n}$ は状態ベクトルの次元を示す。そして ${B_t}$ は ${n \times m}$ の行列で、 ${m}$ は制御ベクトルの次元を示す。このように行列 ${A_t、B_t}$ を導入することで、カルマンフィルタは入力である状態と制御の線形関数として状態遷移を表現する。
なお ${\epsilon_t}$ は、状態遷移の不確かさを表現するためのガウスノイズで、平均は ${0}$ 、共分散は ${R_t}$ 、次元は状態ベクトルと同じ ${n}$ とする。

${(1)}$ 式を多変量ガウス分布の式に当てはめると、平均 ${A_t x_{t-1} + B_t u_t}$ で共分散 ${R_t}$ である
${ p(x_t | u_t, x_{t-1}) = det(2 \pi R_t)^{- \frac{1}{2}} exp\{- \frac{1}{2} (x_t - A_t x_{t-1} - B_t u_t)^T R^{-1}_t (x_t - A_t x_{t-1} - B_t u_t)\} \tag{2} }$
として表せる。

【２点目】
計測確率 ${p(z_t | x_t)}$ も、状態遷移確率と同様で入力に対して線形とする。また計測データは、ガウスノイズを加味した下記の式で表せる。
${z_t = C_t x_t + \delta_t \tag{3}}$
ここで ${C_t}$ は ${k \times n}$ の行列で、 ${k}$ は計測ベクトル ${z_t}$ の次元とする。 ${\delta_t}$ は、平均 ${0}$ で共分散 ${Q_t}$ の計測ノイズである。計測確率は、 ${(2)}$ と同様に多変量ガウス分布の形で、
${ p(z_t | x_t) = det(2 \pi Q_t)^{- \frac{1}{2}} exp\{- \frac{1}{2} (z_t - C_t x_t)^T Q^{-1}_t (z_t - C_t x_t)\} \tag{4} }$
として表せる。

【３点目】
最後に、初期belief ${bel(x_0)}$ は、平均 ${\mu_0}$ で共分散 ${\Sigma_0}$ のガウス分布
${ bel(x_0) = p(x_0) = det(2 \pi \Sigma_0)^{- \frac{1}{2}} exp\{- \frac{1}{2} (x_0 - \mu_0)^T \Sigma^{-1}_0 (x_0 - \mu_0)\} \tag{5} }$
として表せる。

これら３つの仮定は、 ${bel(x_t)}$ がガウス分布で表せることを確認するのに十分である。

3.2.2 The Kalman Filter Algorithm

カルマンフィルタの入力と出力は
入力： ${平均\mu_{t-1}、共分散\Sigma_{t-1}のbel(x_{t-1})}$ と制御ベクトル ${u_t}$ 、観測ベクトル ${z_t}$
出力： ${平均\mu_t、共分散\Sigma_tのbel(x_t)}$
である。アルゴリズムは下記の数式で表せる。

${ Algorithm \ Kalman\_Filter(\mu_{t-1}, \Sigma_{t-1}, u_t, z_t):\\ \ \ \overline{\mu}_t = A_t \mu_{t-1} + B_t u_t\\ \ \ \overline{\Sigma}_t = A_t \Sigma_{t-1} A^T_t + R_t\\ \\ \ \ K_t = \overline{\Sigma}_t C^T_t (C_t \overline{\Sigma}_t C^T_t + Q_t)^{-1} \\ \ \ \mu_t = \overline{\mu}_t + K_t (z_t - C_t \overline{\mu}_t)\\ \ \ \Sigma_t = (I - K_t C_t) \overline{\Sigma}_t\\ \ \ return\ \mu_t, \Sigma_t }$
Table3.1 対象が線形な場合のカルマンフィルタアルゴリズム

なお ${K_t}$ はカルマンゲインと呼ばれ、状態遷移時に観測結果をどの程度考慮するかを調節する値である。

Table3.1の２，３行目は制御を加えた結果の予測過程を、４～６行目は観測結果を元に状態を更新する過程を、それぞれ示している。
それぞれの行に対する私の解釈は、

２行目：制御を加えたことによる平均の変化を予測
３行目：共分散 ${\Sigma_t}$ は分散の２乗であるから、 ${A_t}$ に対する二次形式になっている
５行目：予測した平均に対し、予測からどれだけずれたか、 ${z_t - C_t \overline{\mu_t}}$ を考慮して平均を更新
６行目：共分散を求める式なので、 ${I - K_t C_t}$ の部分は必ず正になる。よって ${0 \ \leq \ (I - K_t C_t)_i \ \leq \ 1}$ とわかる。このことから、共分散を抑えにかかっていると言える。

下記に私がPythonで書いたカルマンフィルタのコードを載せました。参考になれば。

3.3 The Extended Kalman Filter

3.3.1 Why Liearlization?

システムが線形であることは、現実的にはかなり稀である。そのため、本節では非線形なシステムにカルマンフィルタを適用する方法を検討する。今状態 ${x_t}$ と観測結果 ${z_t}$ がそれぞれ、
${ x_t = g(u_t, x_{t-1}) + \epsilon_t \\ z_t = h(x_t) + \delta_t }$
で表せるものとする。ここで ${g、h}$ は非線形関数とする。
上記２式は、線形カルマンフィルタで使った行列 ${A_t、B_t}$ を ${g}$ で、行列 ${C_t}$ を ${h}$ にそれぞれ置き換えた式と言える。つまり拡張カルマンフィルタは、基本的には線形カルマンフィルタと同じと言える。ただ一方で、非線形関数を用いることからbeliefはガウス分布の形を成さなくなる。

3.3.2 Linealization Via Taylor Expansion

EKFの重要なアイデアは、線形化である。 ${g、h}$ を線形化すると、EKFはLKFと同様にして振る舞う。
線形化には種々の方法があるが、EKFでは１次のテイラー展開[2]を用いる。よって ${g、h}$ はそれぞれ、

${ g(u_t, x_{t-1}) \approx g(u_t, \mu_{t-1}) + g^{'}(u_t, \mu_{t-1})(x_{t-1} - \mu_{t-1})\\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ = g(u_t, \mu_{t-1}) + G_t (x_{t-1} - \mu_{t-1})\\ \\ h(x_t) \approx h(\overline{\mu_t}) + h^{'}(\overline{\mu_t})(x_t - \overline{\mu_t})\\ \ \ \ \ \ \ \ \ \ = h(\overline{\mu_t}) + H_t (x_t - \overline{\mu_t}) }$

と近似できる。この結果を使い、状態遷移確率 ${p(x_t | u_t, x_{t-1})}$ と観測確率 ${p(z_t | x_t)}$ はそれぞれ、

${ p(x_t | u_t, x_{t-1}) \approx det(2 \pi R_t)^{- \frac{1}{2}} exp \{ - \frac{1}{2} [x_t - g(u_t, \mu_{t-1}) - G_t (x_{t-1} - \mu_{t-1} ]^T R^{-1}_t [x_t - g(u_t, \mu_{t-1}) - G_t (x_{t-1} - \mu_{t-1} ] \} \\ \\ p(z_t | x_t) = det(2 \pi R_t)^{- \frac{1}{2}} exp \{- \frac{1}{2} [ z_t - h(\overline{\mu}_t - H_t (x_t - \overline{\mu}_t) ]^T Q^{-1}_t [ z_t - h(\overline{\mu}_t - H_t (x_t - \overline{\mu}_t) ] \} }$

と表すことができる。

${ Algorithm \ Extended_Kalman\_Filter(\mu_{t-1}, \Sigma_{t-1}, u_t, z_t):\\ \ \ \overline{\mu}_t = g(u_t, \mu_{t-1})\\ \ \ \overline{\Sigma}_t = G_t \Sigma_{t-1} G^T_t + R_t\\ \\ \ \ K_t = \overline{\Sigma}_t H^T_t (H_t \overline{\Sigma}_t H^T_t + Q_t)^{-1} \\ \ \ \mu_t = \overline{\mu}_t + K_t (z_t - h(\overline{\mu}_t))\\ \ \ \Sigma_t = (I - K_t H_t) \overline{\Sigma}_t\\ \ \ return\ \mu_t, \Sigma_t }$

3.4 The Unscented Kalman Filter(UKF)

UKFも非線形なシステムに対応するカルマンフィルタの１手法。線形回帰を使うことで、統計的に線形化する。回帰に使うサンプル点として、Sigma Point[3]を設定する。Sigma Pointは、平均を中心とし、かつ共分散行列の主軸に対して線対称になるように配置する。平均 ${\mu}$ ・共分散 ${\Sigma}$ な ${n}$ 次元のガウス分布については、 ${2n+1}$ 個のSigma Pointを配置する。配置時のルールは下記の通り。

${ \begin{align} \chi^{[0]} &= \mu\\ \chi^{[i]} &= \mu + (\sqrt{(n + \lambda \Sigma})_i \ \ \ for\ \ i = 1,...,n \\ \chi^{[i]} &= \mu - (\sqrt{(n + \lambda \Sigma})_{i-n} \ \ \ for\ \ i = n+1,...,2n \\ \end{align} }$

ここで、 ${\lambda = \alpha^2(n + \kappa) - n}$ で、 ${\alpha}$ と ${\kappa}$ は ${\mu}$ からどの程度離れるかを決めるスケールパラメータである。

また、各 ${\chi^{[i]}}$ は２種類の重み ${w^{[i]}_m}$ と ${w^{[i]}_c}$ を持つ。 ${w^{[i]}_m}$ は平均を、 ${w^{[i]}_c}$ はガウス分布の共分散を、それぞれ計算する際に用いる。なお ${w^{[i]}_m}$ と ${w^{[i]}_c}$ は下記の式で求める。

${ \begin{align} w^{[0]}_m &= \frac{\lambda}{n + \lambda} \\ w^{[0]}_c &= \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta) \\ w^{[i]}_m &= w^{[i]}_c = \frac{1}{2(n + \lambda)} \ \ \ for\ \ i = 1,...,2n. \end{align} }$

ここで ${\beta}$ は、分布に関する追加情報をエンコードするためのパラメータであり、分布がガウス分布であれば ${\beta = 2}$ が最良となる。
　Sigma Pointは、関数 ${g}$ を使って写像される。写像後の平均 ${\mu^{'}}$ と共分散 ${\Sigma^{'}}$ はそれぞれ、
${ Y^{[i]} = g(\chi^{[i]}) \\ \displaystyle \mu^{'} = \sum^{2n}_{i=0} w^{[i]}_m Y^{[i]}\\ \Sigma^{'} = w^{[i]}_c (Y^{[i]} - \mu^{'} ) (Y^{[i]} - \mu^{'} )^T }$
として表せる。

3.4.2 The UKF Alogorithm

UKFとEKFの入出力は同じだが、計算負荷はUFKの方が少し高い。また線形システムでは、UFKとLKFは同じ結果を示す。UKFの利点は、ヤコビアンの計算が不要な点である。ヤコビアンの計算には微分が必要であり、時には計算が難しい。そのためヤコビアンの計算を回避できることは計算上好ましい。なおUKFはパーティクルフィルタに似ているが、パーティクルフィルタはサンプル点をランダムに取るのに対し、UKFではサンプル点の取り方が固定という違いがある。

${ Algorithm\ \ Unscented\_Kalman\_Filter(\mu_{t-1}, \Sigma_{t-1}, u_t, z_t):\\ \ \ \ \chi_{t-1} = (\mu_{t-1} \ \ \ \mu_{t-1} + \gamma \sqrt{\Sigma_{t-1}} \ \ \ \mu_{t-1} - \gamma \sqrt{\Sigma_{t-1}})\\ \ \ \ \overline{\chi}^{*}_t = g(u_t, \chi_{t-1})\\ \ \ \ \overline{\mu}_t = \sum^{2n}_{i=0} w^{[i]}_m \overline{\chi}^{*[i]}_t \\ \ \ \ \overline{\Sigma}_t = \sum^{2n}_{i=0} w^{[i]}_c (\overline{\chi}^{*[i]}_t - \overline{\mu}_t) (\overline{\chi}^{*[i]}_t - \overline{\mu}_t)^T + R_t \\ \ \ \ \overline{\chi}_t = (\overline{\mu}_t \ \ \ \overline{\mu}_t + \gamma \sqrt{\overline{\Sigma}_t} \ \ \ \overline{\mu}_t - \gamma \sqrt{\overline{\Sigma}_t})\\ \ \ \ \overline{Z}_t = h( \overline{\chi}_t)\\ \ \ \ \hat{z}_t = \sum^{2n}_{i=0} w^{[i]}_m \overline{Z}^{[i]}_t \\ \ \ \ S_t = \sum^{2n}_{i=0} w^{[i]}_c (\overline{Z}^{[i]}_t - \hat{z}_t) (\overline{Z}^{[i]}_t - \hat{z}_t)^T + Q_t \\ \ \ \ \sum^{x,z}_t = \sum^{2n}_{i=0} w^{[i]}_c (\overline{\chi}^{[i]}_t - \overline{\mu}_t) (\overline{Z}^{[i]}_t - \overline{\mu}_t)^T \\ \ \ \ K_t = \sum^{x,z}_t S^{-1}_t \\ \ \ \ \mu_t = \overline{\mu}_t + K_t ( z_t - \hat{z}_t )\\ \ \ \ \Sigma_t = \overline{\Sigma}_t - K_t S_t K_t^T \\ \ \ \ return \ \ \mu_t, \Sigma_t }$

3.5 The Information Filter（IF)

　LKFと同様に線形なシステムを仮定している。LKFとの主な違いは、ガウス分布の表現方法である。LKFとその派生は、ガウス分布をモーメント（平均と共分散）を使って表している。それに対しIFは、カノニカルな表現方法を使用する（＊注意：僕、カノニカルとはどういうことかよくわかっていません！調べている途中です。。）。

　IFとLKFは、状態遷移確率と観測確率それぞれを、LKFと同じ式で求める。またLKFと同様に、予測と更新の２ステップで構成される。IFの入力は、カノニカルなパラメータ ${\xi_{t-1}と\Omega_{t-1}、u_t、z_t}$ である。そして出力は ${\xi_tと\Omega_t}$ である。なお ${\xiと\Omega}$ の定義は下記の通り。
${ \begin{align} \Omega &= \Sigma^{-1} \\ \xi &= \Sigma^{-1} \mu \end{align} }$

IFとLKF・その派生を比較した時、下記のようなメリット・デメリットがある。

メリット

LKF・その派生よりも数値的に安定している（安定が曖昧だが、他のことばへの言い換えがわからない。。）
LKF・その派生よりもロボットの実問題へ当てはめやすい

デメリット

更新時に状態推定をリカバーする必要がある
状態空間の次元が大きくなると計算コストが膨大になる

参考

[0]Probabilistic Robotics
[1]Filter (signal processing) - Wikipedia
[2]テイラー展開 - Wikipedia
[3]http://lab.cntl.kyutech.ac.jp/~nishida/lecture/psc/no8.pdf

2017-04-27

Numpyでの行列の扱い方

python

Numpyで行列を作る場合、下記の３種類の方法がある。

①numpy.array([1, 2], [3, 4])
②numpy.matrix(numpy.array([1, 2], [3, 4]))
③numpy.mat(numpy.array([1, 2], [3, 4]))

上記の挙動について調べたことを下記に示す。

足し算・引き算

import numpy as np

a = np.array([2, 2])
a.shape -> (2, )

b = np.array([[1, 2], [3, 4]])
b.shape -> (2, 2)

a - b   -> array([[1 , 0],
                  [-1, -2]]) 
# なんと計算できちゃいます。aの１行目が２行目にもコピーされ、
# ２行２列の行列として扱われます。

c = np.matrix([[1, 2, 3], [4, 5, 6]])
c.shape -> (2, 3)

a - c   -> error! # 相手のデータタイプがmatrixだと、aとcのデータタイプが異なるからなのか、aは２行２列とは扱わないです。

掛け算

①の方法を採用する時、行列aとbの掛け算はnumpy.dot(a, b)とする必要がある。もしもa * bとすると、それは各要素毎に掛け算を行うことを意味する。②、③の時は、a * bでもnumpy.dot(a, b)のどちらでもよい。以上をコードで表すと下記の様になる。

import numpy as np

a = np.array([[1, 2], [3, 4]])
b = np.array([[4, 3], [2, 1]])

a * b        -> array([[4 , 6]
                       [6 , 4]]) # a * b[0][0] = a[0] * b[0] となっている
np.dot(a, b) -> array([[8 , 5],
                       [20, 13]])

c = np.matrix(np.array([[1, 2], [3, 4]]))
d = np.matrix(np.array([[4, 3], [2, 1]]))

c * d        -> matrix([[8 , 5],
                        [20, 13]])
np.dot(c * d)-> matrix([[8 , 5],
                        [20, 13]])

e = np.mat(np.array([[1, 2], [3, 4]]))
f = np.mat(np.array([[4, 3], [2, 1]]))

e * f        -> matrix([[8 , 5],
                        [20, 13]])
np.dot(e * f)-> matrix([[8 , 5],
                        [20, 13]])

g = np.array([1, 2])
g * a        -> error!
np.dot(a, a) -> error!
# 掛け算の場合、足し算・引き算と異なり、１行目は２行目にコピーされない

2017-04-21

RTAB-Mapについてのメモ

最近３DSLAMに興味があり、ROSにも対応したRTAB-Mapをいじってみました。その過程でわかったことなどをメモします。ある程度纏まった量が出てくるまでは箇条書きになりますのでご容赦を(笑)

・Eigen3.3.3には対応せず。3.2.0はOK(RTAB-Map 0.12.5で）

2016-07-03

Huawei P9 Liteでアプリによる通知を有効化する方法

先日Huawei P9 Liteに機種変しました。その後Wuderlist等のアプリをインストール・使ったところ、リマインダーの通知が一向に上がってこないという事態に陥りました。調べたところ、P9 Liteのバッテリーマネージャが原因だということがわかりました。本記事では、どうやって通知を有効化すればよいか、設定方法を記載します。

（少なくともP9 Liteでは）、一部のアプリを除き、画面オフ状態やそのアプリが表示されていない時（バックグラウンドでの実行状態）になるとアプリの動作が無効化されるみたいです。そのため、通知を有効化するためには、アプリの動作有効化が必要となります。有効化の方法は単純で、
　　「設定」→「詳細設定」→「バッテリーマネージャ」→「保護されたアプリ」
で有効/無効を、タップで切り替えるだけです。

上記「保護されたアプリ」の一覧でwunderlistを有効化することで、リマインダの通知が上がってくるようになりました。参考になれば幸いです。