2
EKF-SLAM更新ステップ、カルマンゲインが特異になる
SLAMにEKFを使用していますが、更新手順に問題があります。Kが特異であり、とrcond評価されるという警告が表示されnear eps or NaNます。問題の原因はZの反転にあると思います。最後の項を反転せずにカルマンゲインを計算する方法はありますか? 私はこれが問題の原因であるという100%の肯定的ではないので、コード全体をここに入れました。メインエントリポイントはslam2dです。 function [ x, P ] = expectation( x, P, lmk_idx, observation) % expectation r_idx = [1;2;3]; rl = [r_idx; lmk_idx]; [e, E_r, E_l] = project(x(r), x(lmk_idx)); E_rl = [E_r E_l]; E = E_rl * P(rl,rl) * E_rl'; % innovation z = observation - e; Z …