從程式中學習EKF SLAM（二）

進入主迴圈的第乙個部分，就是儲存當前狀態下的資料。這裡順帶提一下系統裡的資料格式。首先看資料離線化儲存的方法：

state temp;
offline_data.i = 0;
offline_data.estimate_path.push_back(x);
offline_data.real_path.push_back(xtrue);
temp.x = x;
temp.p = p.diagonal();//獲取對角線上的元素
<< p(0, 0), p(1, 1), p(2, 2);
offline_data.state.push_back(temp);

這裡的offine_data和state是一種自定義的資料格式，在tool.h檔案裡有儲存：

typedef struct state_data
state;
typedef struct stored_data
sdata;
sdata offline_data;//offline database.

資料儲存的中的x和p資料就是本系統需要維護的重要資料，其中狀態向量的構成如下圖所示：

那麼，這個x也就是ekf中的均值μ

\muμ，以及p也就是協方差∑

\sum

∑。當系統有n個landmark，他們就是是3+2n維的高斯分布，表示如下：

進行簡化一下就是：

這裡通過**器計算觀測資訊，新增雜訊後送入ekf濾波器，那麼，這裡先看看真實位姿的計算。個人覺得它的計算思想特別巧妙，可以熟悉熟悉程式設計方法。

首先，這裡貼出**：

void ekf_console::compute_steering()
current_wp = waypoints.col(iwp);//next waypoint } 
//compute change in g to point towards current waypoint
double deltag=tool::normalize_angle( (atan2( current_wp(1) - ekf_cal->xtrue(1), current_wp(0) - ekf_cal->xtrue(0) ) - ekf_cal->xtrue(2) - g) );
//limit rate
double maxdelta=rateg*dt;
if(abs(deltag)>maxdelta)
deltag= maxdelta*(sign(deltag));
g=g+deltag;
if(abs(g)>maxg)
g=maxg*(sign(g));
}

解釋一下它的過程：

（1）首先，計算car真實位姿xtrue與當前路徑點current_wp（注意car未到這個路徑點）的歐幾里得距離，其中current_wp通過路徑點代號iwp進行索引；

（2）然後，判斷他們的距離是否小於（at_waypoint*at_waypoint），若是則說明達到路徑點附近了，開始更換下乙個路徑點。此處會檢查路徑點代號是否超過最大值，若超過了則置-1。

（3）緊接著，計算（前面的）路徑點與當前位姿的夾角deltag。這裡程式進行了最大值限制，最後累加到轉角g中，完成轉角的更新。

接下來，程式進行了乙個簡單的回環檢測，下面就說說其中涉及的回環檢測。

在視覺slam問題中，位姿的估計往往是乙個遞推的過程，即由上一幀位姿解算當前幀位姿，因此其中的誤差便這樣一幀一幀的傳遞下去，也就是我們所說的累計誤差。

我們的位姿約束都是與上一幀建立的，第五幀的位姿誤差中便已經積累了前面四個約束中的誤差。但如果我們發現第五幀位姿不一定要由第四幀推出來，還可以由第二幀推算出來，顯然這樣計算誤差會小很多，因為只存在兩個約束的誤差了。像這樣與之前的某一幀建立位姿約束關係就叫做回環。回環通過減少約束數，起到了減小累計誤差的作用。

那麼我們怎麼知道可以由第二幀推算第五幀位姿呢？也許第一幀、第三幀也可以呢。確實，我們之所以用前一幀遞推下一幀位姿，因為這兩幀足夠近，肯定可以建立兩幀的約束，但是距離較遠的兩幀就不一定可以建立這樣的約束關係了。找出可以建立這種位姿約束的歷史幀，就是回環檢測。

———摘自**slam的回環檢測技術

而在這個project中，就是通過簡單地判斷目前到達的路徑點是否回到原點，若是回到原點，則回環次數number_loops減1，若number_loops次數用完則置iwp為0，退出程式。路徑點iwp的更新發生在compute_steering()函式中，回環檢測部分**如下：

if((iwp==-1)&&(number_loops>1))

更新最新路徑點之後，就是需要開始更新真實位姿了，這裡非常好理解，看**：

void extend_kf::vehicle_model(double velocity, double sangle, double dt)

void extend_kf::add_control_noise(double v, double g, int flag)
}

這裡是對真實線速度v和角速度g增加了正態分佈雜訊，由正態分佈隨機數發生器配合**雜訊q產生。

從程式中學習EKF SLAM（二）

從程式中學python

從XML中學習HTML

從sysbench中學習Lua

從程式中學習EKF SLAM（二）

從程式中學python

從XML中學習HTML

從sysbench中學習Lua

相關推薦