2
EKF-SLAM更新步骤,卡尔曼增益变得奇异
我正在为SLAM使用EKF,并且更新步骤遇到了一些问题。我收到警告,说K奇异,rcond计算为near eps or NaN。我想我已经将问题追溯到Z的倒数了。有没有一种方法可以计算出Kalman增益而不倒数最后一项? 我不是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 …