SLAM系列(二)：擴(kuò)展卡爾曼濾波SLAM理論部分a

關(guān)于2維的SLAM我們主要講解兩個(gè)算法，基本都源于<probabilistic robotics>。第一是基于濾波的擴(kuò)展卡爾曼濾波SLAM盟猖，第二是基于圖優(yōu)化SLAM讨衣。前者現(xiàn)在基本上已經(jīng)沒有實(shí)際使用了，實(shí)際的應(yīng)用3d的SLAM也基本是基于后者的式镐。但擴(kuò)展卡爾曼濾波SLAM還是很經(jīng)典反镇，值得學(xué)習(xí)。
從名字可以看出娘汞，擴(kuò)展卡爾曼濾波(Extended Kalman Filter歹茶，以后我們就簡寫為EKF了)SLAM是基于擴(kuò)展卡爾曼濾波算法的。而EKF又是基于卡爾曼濾波(Kalman Filter价说，我們簡寫維KF)的辆亏。所以自然要了解EKF-SLAM我們就先得了解EKF,KF。首先必須強(qiáng)調(diào)的是卡爾曼濾波應(yīng)用范圍很廣鳖目，不單單是SLAM扮叨，雖然我們這兒就針對(duì)SLAM講解。
卡爾曼濾波是用來做什么的呢?回顧我們在第一講的基本原理部分的Question1中發(fā)出的靈魂提問领迈，通過速度傳感器和時(shí)間我們能算出 $|BC|_1$ 彻磁，通過先知道 $|AC|,|AB|$ 等我們能算出 $|BC|_2$ ，這兩個(gè)值由于傳感器的不準(zhǔn)確和我們對(duì)小車勻速運(yùn)動(dòng)這個(gè)假設(shè)不準(zhǔn)確而不同狸捅，并且都有別于真實(shí)值衷蜓。所以最終我們應(yīng)該相信哪一個(gè)？最簡單的想法兩種方法算出的 $|BC|$ 求和再除以2尘喝，這樣我們兩種方法所得到的值都使用了磁浇。用式子表達(dá)就是
$|BC| = a * |BC|_1 + b * |BC|_2$
此時(shí)取 $a = b = 0.5$
但這樣真的好嗎？假如小車對(duì) $|AB|,|AC|$ 的測量非常不準(zhǔn)確朽褪，最終使誤差達(dá)到了0.1m置吓，而通過另一種方法由于速度傳感器比較準(zhǔn)確，我們得到誤差0.01m缔赠，很明顯這時(shí)候我們更寧愿相信 $|BC|_1$ 而不是 $|BC|_2$ 衍锚。讓最終的結(jié)果取 $|BC|_1，|BC|_2$ 各占一半顯然不好嗤堰，應(yīng)該讓 $|BC|_1$ 占個(gè)0.9戴质，讓 $|BC|_2$ 占個(gè)0.1更好。具體這個(gè)a,b踢匣，即我們俗稱的權(quán)重該怎么取值告匠，就是卡爾曼濾波要解決的問題了。

卡爾曼濾波

在了解卡爾曼濾波之前离唬，我們必須要了解兩個(gè)東西凫海，過程模型(process model)和測量模型(observation model)。用數(shù)學(xué)表達(dá)式寫來如下男娄。
$\begin{align} \mathbf{x}_k &= \mathbf{F}_k \mathbf{x}_{k-1} + \mathbf{B}_k \mathbf{u}_k + \mathbf{w}_k \\ \mathbf{z}_k &= \mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k \end{align} \tag{1}$
了解卡爾曼濾波的同學(xué)自不必說，不了解狀態(tài)空間或者狀態(tài)空間的同學(xué)估計(jì)要崩了，之前還講的那么淺顯易懂的內(nèi)容模闲，現(xiàn)在突然就冒出來一大堆莫名其妙的公式建瘫，什么鬼？尸折？啰脚？

其實(shí)這兩個(gè)式子描述了兩個(gè)很簡單的東西，下面我細(xì)細(xì)道來实夹。
第一個(gè)式子中橄浓， $k$ 是每一個(gè)時(shí)刻的代號(hào)，那么 $k-1$ 就是上個(gè)時(shí)刻的代號(hào)亮航。 $\mathbf{x}_{k-1}$ 就表示上一個(gè)時(shí)刻的狀態(tài)荸实，其中 $\mathbf{x}$ 表示的就是小車或者機(jī)器人狀態(tài),比如位置，方向等缴淋。 $\mathbf{F}_k$ 被稱為狀態(tài)轉(zhuǎn)移矩陣( $\; State \; Transition \; Matrix$ )准给。它連接上一時(shí)刻的狀態(tài)和當(dāng)前時(shí)刻的狀態(tài)。我們來舉個(gè)栗子重抖。如果有小車作一個(gè)一維的直線運(yùn)動(dòng)露氮，從位置A運(yùn)動(dòng)到位置B，我們估計(jì)它做勻速直線運(yùn)動(dòng)钟沛，它的狀態(tài)包含速度和位置(狀態(tài)是人為定的畔规，如果你感興趣我們可以把加速度什么的也包含進(jìn)去，不過這兒沒必要了)恨统，它在 $k-1$ 時(shí)刻的狀態(tài)記為 $\mathbf{x}_{k-1} = ( p_{k-1}, v_{k-1})$ 叁扫。由于小車做勻速直線運(yùn)動(dòng)我們知道他下一時(shí)刻的位置和速度可以估計(jì)為
$\begin{align} & p_{k} = p_{k-1} + v_{k-1} * \Delta t \\ & v_{k} = v_{k-1} \end{align}$
寫成矩陣形式，就是
$\begin{align} \begin{bmatrix} p_{k} \\ v_{k} \end{bmatrix} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} p_{k-1} \\ v_{k-1} \end{bmatrix} \end{align} \tag{2}$
那么一目了然狀態(tài)轉(zhuǎn)移矩陣就是那個(gè)方陣了延欠。
$\begin{align} \mathbf{F}_k = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \end{align}$
有的同學(xué)可能要問了陌兑，狀態(tài)轉(zhuǎn)移矩陣后面還有兩個(gè)東西呢 $\mathbf{B}_k \mathbf{u}_k$ ，外加 $\mathbf{w}_k$ 由捎。其中 $\mathbf{u}_k$ 是控制輸入(control input)兔综， $\mathbf{B}_k$ 是輸入控制模型(control input model)。比如我們想控制小車的速度而在 $k-1$ 時(shí)刻人為增加了1倍的當(dāng)前速度狞玛，對(duì)位置不做改變软驰，那么我們完整的狀態(tài)預(yù)測公式就改寫為
$\begin{align} \begin{bmatrix} p_k \\ v_k \end{bmatrix} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} p_{k-1} \\ v_{k-1} \end{bmatrix} + \begin{bmatrix} 0 & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 0 \\ v_{k-1} \end{bmatrix} \end{align}$
既然 $\mathbf{B}_k \mathbf{u}_k$ 屬于人為的控制，那么我們可以選擇讓他們?nèi)繛?心肪，即不添加任何人為的控制干預(yù)锭亏，讓狀態(tài)任其發(fā)展，這也是很常見的硬鞍。 $\mathbf{w}_k$ 就是噪音了慧瘤，我們假設(shè)小車是勻速直線運(yùn)動(dòng)戴已，但是我們知道沒有絕對(duì)的勻速直線運(yùn)動(dòng)，于是我們對(duì)估計(jì)的值加上了噪音來表示實(shí)際的位置和我們完美的勻速直線運(yùn)動(dòng)可能有差別锅减。這個(gè)噪音通常建模為符合是均值為0的正態(tài)分布噪音糖儡。如果我們加上噪音，那么我們完整的過程模型就寫為
$\begin{align} \begin{bmatrix} p_k \\ v_k \\ \end{bmatrix} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} p_{k-1} \\ v_{k-1} \end{bmatrix} + \begin{bmatrix} 0 & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 0 \\ v_{k-1} \end{bmatrix} + \begin{bmatrix} w_1 \\ w_2 \end{bmatrix} \end{align} \tag{3}$
其中
$\begin{align} w_1 \sim \mathcal{N}(0, var_1)\\ w_2 \sim \mathcal{N}(0, var_2) \end{align}$
$var_1 , var_2$ 為正態(tài)分布的方差怔匣。這樣一個(gè)有人為輸入的握联，有噪音的勻速直線運(yùn)動(dòng)小車模型就建立好了。
回到(1)式中的第二個(gè)式子每瞒，被稱為觀察模型金闽，這個(gè)就更簡單了。其中 $\mathbf{H}_k$ 被稱為觀察模型矩陣剿骨，它表示我們測量量和我們需要的狀態(tài)之間的關(guān)系代芜。比如我們現(xiàn)在有速度傳感器可以測小車的位置，但是沒有速度傳感器可以測小車的速度懦砂，那么我們的觀察模型就應(yīng)該如下寫蜒犯。
$z_k = [1 \ \ \ 0] \begin{bmatrix} p_k \\ v_k \end{bmatrix} + v_1$
和式(1)中的每個(gè)量對(duì)應(yīng)起來，顯而易見 $\mathbf{H} = [1 \ \ \ 0], \mathbf{v}_k = v_1$ 荞膘。由于測量肯定也是不會(huì)完全準(zhǔn)確的罚随，所以我們對(duì)位置的測量結(jié)果在真實(shí)位置的基礎(chǔ)之上也加了一個(gè)噪音，同樣這個(gè)噪音我們建模為均值為0羽资，方差維 $var_3$ 的高斯分布淘菩。即
$v_1 \sim \mathcal{N}(0,var_3)$
有了式(2)，我們相當(dāng)于就對(duì)小車每時(shí)每刻的速度位置以及傳感器會(huì)給出的值成功建模屠升，理論上說這就是我們對(duì)一個(gè)系統(tǒng)狀態(tài)所需要了解的一切了潮改。上面兩個(gè)式子，是對(duì)真實(shí)系統(tǒng)的模擬腹暖，如果我們想利用代碼中估計(jì)系統(tǒng)的狀態(tài)汇在，我們就得想辦法解決那不可預(yù)測的高斯噪音。這就需要卡爾曼濾波的幫助脏答。
現(xiàn)在我先把注明的卡爾曼濾波公式從wiki搬過來糕殉。KF一般分為兩個(gè)大步，第一步叫Predict預(yù)測殖告，第二步叫Update更新阿蝶。(一些小bb:每一步的名字我還是用英文打出來，因?yàn)閷W(xué)術(shù)上大家應(yīng)該更多接觸英文黄绩。解釋時(shí)會(huì)翻譯回中文羡洁。本來可以直接對(duì)wiki寫好的公式截圖的，但是為了方便文章文字內(nèi)容插入的數(shù)學(xué)符號(hào)和整段公式的一致爽丹，我還是自己一個(gè)一個(gè)用latex打出來了筑煮。太麻煩了...感覺以后回因?yàn)椴幌氪蚬蕉艞壒?br> $\begin{align} & \mathbf{Predict} \\ & \; State \, Prediction \; : \widehat{\mathbf{x}}_{k|k-1} = \mathbf{F}_k \widehat{\mathbf{x}}_{k-1|k-1} + \mathbf{B}_k \mathbf{u}_k \\ & \; Error \, Covariance \, Prediction : \mathbf{P}_{k|k-1} = \mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^T+ \mathbf{Q}_k \\ &\mathbf{Update} \\ & \; Kalman \, Gain : \mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T+ \mathbf{R}_k )^{-1} \\ & \; State \, Updation : \widehat{\mathbf{x}}_{k|k} = \widehat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \widehat{\mathbf{x}}_{k|k-1}) \\ & \; Error \, Covariance \, Updation : \mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k ) \mathbf{P}_{k|k-1} \end{align}$

咱么一個(gè)一個(gè)來講解辛蚊。
第一個(gè)公式 $\; State \, Prediction$ 狀態(tài)預(yù)測。會(huì)根據(jù)上一時(shí)刻估計(jì)到的狀態(tài)和運(yùn)動(dòng)模型來估計(jì)當(dāng)前時(shí)刻的狀態(tài)咆瘟。
$\widehat{\mathbf{x}}_{k|k-1} = \mathbf{F}_k \widehat{\mathbf{x}}_{k-1|k-1} + \mathbf{B}_k \mathbf{u}_k$
和前面一樣嚼隘， $k$ 是每一個(gè)時(shí)刻的代號(hào)，那么 $k-1$ 就是上個(gè)時(shí)刻的代號(hào)袒餐。 $\widehat{\mathbf{x}}_{k-1|k-1}$ 就表示上一個(gè)時(shí)刻估計(jì)到的狀態(tài)，其中 $\mathbf{x}$ 表示的就是小車或者機(jī)器人狀態(tài),比如位置谤狡，方向等灸眼。 $\mathbf{x}$ 頭山的這個(gè)" $\widehat{}$ "符號(hào)表示我們的狀態(tài)是估計(jì)值，每一個(gè) $\mathbf{x}$ 頭上都有這個(gè)帽子符號(hào)墓懂，因?yàn)槲覀內(nèi)魏螘r(shí)候的狀態(tài)都是估計(jì)值不是真實(shí)值焰宣。 $\mathbf{x}$ 的下標(biāo)是兩個(gè) $k-1$ ，表示這個(gè) $\mathbf{x}$ 是上一時(shí)刻"最終"得到的狀態(tài)捕仔。我們可以看到除了在第一步獲得的 $\widehat{\mathbf{x}}_{k|k-1}$ 外我們還會(huì)在 $\; State \, Updation$ 那一步獲得一個(gè) $\widehat{\mathbf{x}}_{k|k}$ 匕积。也就是說每一次使用卡爾曼濾波我們會(huì)更新狀態(tài)兩次，其中 $\widehat{\mathbf{x}}_{k|k}$ 就會(huì)作為當(dāng)前時(shí)刻估計(jì)的“最終狀態(tài)”被下一時(shí)刻作為 $\widehat{\mathbf{x}}_{k-1|k-1}$ 使用榜跌。為了區(qū)分這兩次更新闪唆，我們把 $\; State \, Prediction \;$ 這一步更新的狀態(tài)記為 $\widehat{\mathbf{x}}_{k|k-1}$ 。后面的例子會(huì)讓大家更明了钓葫。
其實(shí)細(xì)致看來悄蕾，這個(gè)式子和我們公式一中的過程模型基本一樣，只是沒有了噪音础浮。是的帆调，在這一步我們假設(shè)系統(tǒng)完全符合我們預(yù)設(shè)的模型在運(yùn)動(dòng)。而我們對(duì)噪音的處理方法體現(xiàn)在下一個(gè)式子 $\; Error \, Covariance \, Prediction$ 誤差協(xié)方差預(yù)測豆同。

第二個(gè)式子 $\; Error \, Covariance \, Prediction$ 誤差協(xié)方差預(yù)測番刊。
$\mathbf{P}_{k|k-1} = \mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^T+ \mathbf{Q}_k$
如果不了解協(xié)方差是什么的同學(xué)可以隨意百度/google一下。誤差協(xié)方差 $\mathbf{P}$ 是用來對(duì)狀態(tài)估計(jì)準(zhǔn)確度的一種測量影锈。我們可以看到這一步更新的誤差協(xié)方差矩陣的下標(biāo)是 $k|k-1$ 芹务，同時(shí)在 $\; Error \, Covariance \, Updation$ 那一步我們又更新了 $\mathbf{P}$ ，其下標(biāo)是 $k|k$ 精居。我們可以先簡單地記憶為锄禽，由于誤差協(xié)方差矩陣是對(duì)狀態(tài) $\mathbf{x}$ 準(zhǔn)確度的測量，那么它就要和我們的狀態(tài)的下標(biāo)的更新一一對(duì)應(yīng)靴姿。
這個(gè)式子中 $\mathbf{P}_{k-1|k-1}$ 是上一步在 $\; Error \, Covariance \, Updation$ 處更新的協(xié)方差矩陣沃但， $\mathbf{Q}_k$ 是被稱為過程噪音協(xié)方差矩陣(process noise covariance)，它體現(xiàn)我們對(duì)模型準(zhǔn)確度的一種估計(jì)佛吓。比如我們對(duì)小車勻速直線運(yùn)動(dòng)這個(gè)模型不是百分之百的自信宵晚，考慮到各種環(huán)境因素我們覺得式(2)中的位置估計(jì)的公式我們要加個(gè)0.1的方差來描述它的不確定程度垂攘，而速度估計(jì)的公式我們要加個(gè)0.01的方差來描述它的不確定程度，這時(shí)候我們就應(yīng)該把 $\mathbf{Q}_k$ 寫為下面的樣子淤刃。
$\mathbf{Q}_k = \begin{bmatrix} 0.1 & 0 \\ 0 & 0.01 \\ \end{bmatrix}$
當(dāng)我們這樣寫下 $\mathbf{Q}_k$ 時(shí)晒他，意味著我們認(rèn)為某一時(shí)刻估計(jì)位置 $\widehat{p}_{k|k}$ 的估計(jì)是符合均值為實(shí)際位置 $p_{k}$ ，方差為0.1的正態(tài)分布逸贾，估計(jì)速度 $\widehat{v}_{k|k}$ 符合均值為實(shí)際速度為 $v_{k}$ 陨仅，方差為0.01的正態(tài)分布即
$\widehat{p}_{k|k} \sim \mathcal{N}(p_{k},0.1) \\ \widehat{v}_{k|k} \sim \mathcal{N}(v_{k},0.01) \tag{4}$
和前面講的噪音符合高斯分布聯(lián)系起來想一下原因...很感興趣的可以參考<Estimiation with Applications to Tracking & Navigation>第一章的1.14以及之前的一些內(nèi)容。 $\mathbf{Q}_k$ 的非對(duì)角線元素如果不為0意味著我們估計(jì)的狀態(tài)之間還有關(guān)聯(lián)铝侵。我在使用中通常只設(shè)置對(duì)角線元素灼伤，如果發(fā)現(xiàn)結(jié)果很不好才考慮設(shè)置非對(duì)角線元素。
你寫入 $\mathbf{Q}_k$ 中的方差要和實(shí)際的模型的方差類似(當(dāng)然一樣更好)咪鲜，這樣卡爾曼濾波才會(huì)有好的效果狐赡，比如上面如果我們以為 $\widehat{p}_{k|k} \sim \mathcal{N}(p_{k|k},0.1)$ 而實(shí)際的模型方差應(yīng)該是1，那么我們最后得到的狀態(tài)估計(jì)結(jié)果可能就會(huì)很不好疟丙。那么問題來了颖侄，我怎么知道 $\mathbf{Q}_k$ 里該填寫多少呢？或者換句話說享郊，我怎么知道我的模型有多準(zhǔn)確呢览祖？大多數(shù)情況下的答案是
你不能知道
很多人會(huì)根據(jù)卡爾曼濾波輸出的表現(xiàn)去調(diào)整 $\mathbf{Q}_k$ 里的值，或者根據(jù)利用一些算法去幫助我們找到合適的值拂蝎。這一門專門的學(xué)問了穴墅，被稱為卡爾曼濾波參數(shù)整定。我的第一篇論文<Weak in the NEES?: Auto-tuning Kalman Filterswith Bayesian Optimization>即是講述這個(gè)內(nèi)容的温自，第二篇論文還沒出爐玄货，不過是關(guān)于第一篇的應(yīng)用引深。作為新手悼泌，我們給你舉例子時(shí)通常給出了 $\mathbf{Q}_k$ 的值松捉，但是你需要記住的是實(shí)際上要確認(rèn) $\mathbf{Q}_k$ 的值不是那么容易的。
最后說一下為什么 $\; Error \, Covariance \, Prediction$ 公式長這個(gè)樣子呢馆里？ $\mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^T+ \mathbf{Q}_k$ 的物理意義是什么隘世？為什么這么算就可以更新誤差協(xié)方差？.......哪來這么多問題鸠踪！先記妆摺！

問題多.jpeg

如果你的運(yùn)動(dòng)模型能寫為式(2)那種矩陣形式营密，代碼應(yīng)用時(shí)套這個(gè)公式就可以了械媒，還有什么比直接套公式更爽的事呢。如果你想要知道為什么這么寫，可以去搜索一下卡爾曼濾波推導(dǎo)過程纷捞，又是一小波坑等你填我是不想幫你填了痢虹，又是一大堆公式23333。我推薦的是先看看類似于我這種暴力教程加代碼主儡，應(yīng)用一下奖唯，再去了解原理，會(huì)理解更深糜值。

第三個(gè)式子偎快，在update里惫霸，計(jì)算Kalman Gain(卡爾曼增益)楣颠。
$\; Kalman \, Gain : \mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T+ \mathbf{R}_k )^{-1}$
卡爾曼增益的作用况木，就是分配模型預(yù)測的狀態(tài)和傳感器測量的狀態(tài)之間的權(quán)重。公式中 $\mathbf{H}_k$ 被稱為觀測模型矩陣健无，起到連接觀測量和狀態(tài)之間的關(guān)系。接著用我們上面一維小車的例子液斜，如果我們現(xiàn)在有一個(gè)傳感器可以直接測量小車位置累贤，那么我們會(huì)得到公式1觀察模型中的 $\mathbf{z}_k$ 。 $\mathbf{H}_k$ 即前面提到的觀察模型矩陣少漆。需要隆重介紹的是觀察噪音協(xié)方差矩陣 $\mathbf{R}_k$ 臼膏，這個(gè)矩陣和 $\mathbf{Q}_k$ 的性質(zhì)是一樣的，不過代表的是對(duì)測量的值的不準(zhǔn)確度的估計(jì)示损。如果我們知道位置誤差的方差大概是0.1渗磅，那么此時(shí)
$\mathbf{R}_k = 0.1$
$\mathbf{R}_k$ 和 $\mathbf{Q}_k$ 有個(gè)很不同的一點(diǎn)， $\mathbf{Q}_k$ 的值開始你基本全靠猜...但是 $\mathbf{R}_k$ 你卻可以有所了解检访，往往在購買一個(gè)稍微好一點(diǎn)的傳感器時(shí)始鱼，它會(huì)標(biāo)注這個(gè)傳感器會(huì)在什么條件時(shí)產(chǎn)生標(biāo)準(zhǔn)差為多少的噪音，標(biāo)準(zhǔn)差的平方就可以放進(jìn) $\mathbf{R}_k$ 里脆贵。傳感器自身提供的參數(shù)不一定準(zhǔn)確或者適應(yīng)你的環(huán)境医清，但是你至少有一個(gè)初值，可以在這個(gè)值周圍調(diào)整參數(shù)卖氨。
如果你想要知道為什么卡爾曼增益是這樣求会烙，也得去看看卡爾曼濾波的推導(dǎo)過程了。在講完第四個(gè)式子后筒捺，我們來聊一個(gè)很直觀的東西柏腻，來讓你覺得這個(gè)這個(gè)卡爾曼增益還"蠻有道理"的。
第四個(gè)式子系吭，狀態(tài)更新 $\; State \, Updation$ 五嫂。這一步的目的很明了，根據(jù)獲得的卡爾曼增益和測量值對(duì)狀態(tài)進(jìn)一步更新村斟，由 $\widehat{\mathbf{x}}_{k|k-1}$ 到 $\widehat{\mathbf{x}}_{k|k}$ 贫导。
$\; State \, Updation : \widehat{\mathbf{x}}_{k|k} = \widehat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \widehat{\mathbf{x}}_{k|k-1})$
其中 $\mathbf{z}_k$ 是傳感器的測量值抛猫，這是不用估計(jì)的，傳感器顯示多少就是多少孩灯。大家回顧式一可以發(fā)現(xiàn)測量模型為 $\mathbf{z}_k = \mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k$ 闺金。而 $\mathbf{H}_k \widehat{\mathbf{x}}_{k|k-1}$ 是我們預(yù)估的測量值，認(rèn)為真實(shí)狀態(tài)就為 $\widehat{\mathbf{x}}_{k|k-1}$ 的完美情況峰档。如果測量值很不準(zhǔn)確败匹，則代表其方差很大，這會(huì)反映在我們的噪音協(xié)方差矩陣 $\mathbf{R}_k$ 比較大上讥巡，在求卡爾曼增益部分掀亩，咱們可以把求逆換個(gè)寫法
$\mathbf{K}_k = \frac{\mathbf{P}_{k|k-1} \mathbf{H}_k^T }{(\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T+ \mathbf{R}_k )}$
$\mathbf{R}_k$ 在分母部分，它越大欢顷，卡爾曼增益就會(huì)越小槽棍，那么 $(\mathbf{z}_k - \mathbf{H}_k \widehat{\mathbf{x}}_{k|k-1})$ 就會(huì)乘以一個(gè)比較小的值，直白點(diǎn)理解就是我們趨于不相信測量部分抬驴，因?yàn)樗鼨?quán)重比較小炼七。反之 $\mathbf{R}_k$ 小，測量部分權(quán)重則大布持，我們會(huì)比較相信測量部分豌拙。
這兒可能有些同學(xué)會(huì)犯迷糊，上面這些變量都是矩陣题暖，什么叫矩陣比較大/比較小呢按傅？這其實(shí)是對(duì)矩陣求范數(shù)，不了解的同學(xué)可以去百度一下胧卤，不過我們這兒不了解那個(gè)東西也沒關(guān)系唯绍。我們大可以想象如果所有的矩陣都是單個(gè)數(shù)字的特殊情況，能非常直觀了解卡爾曼增益這個(gè)權(quán)重怎么工作的灌侣。
上面我們的推論是噪音協(xié)方差矩陣比較大推捐，我們就會(huì)得到卡爾曼增益比較小，從而偏向于不相信測量部分侧啼。那么如果我們的過程協(xié)方差矩陣比較大牛柒，我們就應(yīng)該偏向于相信測量部分，那么此時(shí)我們的卡爾曼增益應(yīng)該比較大才對(duì)痊乾，我們試一下能不能由已經(jīng)存在的公式得出這個(gè)結(jié)論皮壁。
這兒我們假設(shè)所有矩陣都是一維(一個(gè)數(shù))的情況進(jìn)行一些暴力操作，這時(shí)候矩陣轉(zhuǎn)置沒有效果
$\begin{align} \mathbf{K}_k & = \frac{\mathbf{P}_{k|k-1} \mathbf{H}_k^T }{(\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T+ \mathbf{R}_k )}\\ & = \frac{\mathbf{P}_{k|k-1} \mathbf{H}_k}{ \mathbf{P}_{k|k-1} \mathbf{H}_k^2+ \mathbf{R}_k}\\ & = \frac{\mathbf{H}_k}{\mathbf{H}_k^2+\mathbf{R}_k / \mathbf{P}_{k|k-1}} \end{align}$
這時(shí)候我們把 $\; Error \, Covariance \, Prediction$ 求 $\mathbf{P}_{k|k-1}$ 的式子帶入哪审，我們可以得到
$\frac{\mathbf{H}_k}{\mathbf{H}_k^2+\mathbf{R}_k / \mathbf{P}_{k|k-1}} = \frac{\mathbf{H}_k}{\mathbf{H}_k^2+\mathbf{R}_k /( \mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^T+ \mathbf{Q}_k)}$
這就很明顯了蛾魄， $\mathbf{Q}_k$ 越大， $\mathbf{R}_k /( ... + \mathbf{Q}_k)$ 越小，從而 $\mathbf{K}_k$ 越大滴须，我們會(huì)偏向于相信測量部分舌狗。這完全符合我們對(duì)于權(quán)重的直觀理解，即如果傳感器的誤差大扔水，我們應(yīng)該分配少一些的權(quán)重給傳感器的測量痛侍，如果我們對(duì)運(yùn)動(dòng)模型的估計(jì)誤差大，我們就應(yīng)該分配多一些的權(quán)重給傳感器的測量魔市。盡管我們是在一維的情況下得到這些結(jié)論主届，但這對(duì)高維是完全適用的。
更新完?duì)顟B(tài)后待德，我們就進(jìn)入第五個(gè)式子君丁，更新誤差協(xié)方差矩陣 $\; Error \, Covariance \, Updation \,\mathbf{P}_{k|k}$ ，用于作為下一次迭代的 $\mathbf{P}_{k-1|k-1}$ 将宪。這個(gè)式子沒有啥好聊的了绘闷，大家寫代碼的時(shí)候照著它來即可。
我們介紹完了卡爾曼濾波的一到五式较坛，我想最重要需要了解的信息就是簸喂，它是基于我們假設(shè)的過程噪音(表現(xiàn) $\mathbf{Q}_k$ 中)和測量噪音(表現(xiàn) $\mathbf{R}_k$ 中)為我們的運(yùn)動(dòng)模型獲得的狀態(tài)和傳感器測量得到的狀態(tài)分配一個(gè)權(quán)重。
接下來我們根據(jù)上面的一維小車運(yùn)動(dòng)的例子來寫一寫它的卡爾曼濾波的C++（偽）代碼大概是什么樣子燎潮。并不是完整的代碼，只是給大家一個(gè)直觀的理解扼倘。假設(shè)我們使用Eigen庫來進(jìn)行矩陣操作

Eigen::MatrixXd P = Eigen::MatrixXd::Zero(2);//initial error covariance
Eigen::VectorXd x(2) = Eigen::VectorXd::Zero(2);//initial state
Eigen::VectorXd u(2) = Eigen::VectorXd::Zero(2);; //control input
Eigen::VectorXd z(1);//measurement
//Initialize Q,R,F,H
Eigen::MatrixXd Q(2,2),R(1,1),F(2,2),H(1,2),B(2,2);
Q(0,0) = 0.1;
Q(0,1) = 0;
Q(1,0) = 0;
Q(1,1) = 0.01;

R(1,1) = 0.1;

H(0,0) = 1.0;
H(0,1) = 0.0;

F(0,0) = 1.0;
F(0,1) = 0.1;
F(1,0) = 0;
F(1,1) = 1.0;

B(0,0) = 0;
B(0,1) = 0;
B(1,0) = 0;
B(1,1) = 1;
//Implement Kalman Filter
for(;;){
    u(0) = 0;
    u(1) = x(1);
    z      = GetCurrentMeasurement();

    //Predict
    x = F*x + B*v;
    P = F*P*F.transpose() + Q;
    //Update
    K = P*H.transpose()*(H*P*H.transpose()+R).inverse();
    x = x + K*(z - H*x);
    P = (Eigen::MatrixXd::Identity(2)-K*H)*P;
}

程序非常簡短确封，下面簡要說明一下。程序變量的名字和前面文章講解卡爾曼濾波時(shí)的名字一一對(duì)應(yīng)再菊，比如誤差協(xié)方差矩陣名字叫P爪喘，這兒連下標(biāo)k|k-1都不用了，因?yàn)樵趂or循環(huán)里更新的時(shí)候后一時(shí)刻的狀態(tài)就在計(jì)算時(shí)把前一時(shí)刻狀態(tài)替換了纠拔。
首先初始化誤差協(xié)方差矩陣P為2乘2的零矩陣秉剑。誤差協(xié)方差矩陣對(duì)應(yīng)我們當(dāng)前狀態(tài)的方差，把它設(shè)置為0代表我們對(duì)當(dāng)前的狀態(tài)值有絕對(duì)的把握稠诲。
我們的初始狀態(tài)x隨后設(shè)置為2乘1的0向量侦鹏，代表初始位置和速度都是0。現(xiàn)實(shí)情況中臀叙，即使初始的狀態(tài)我們可能也沒有把握略水，此時(shí)誤差協(xié)方差矩陣也就不一定是0了，而是根據(jù)現(xiàn)實(shí)情況瞎猜一下hhhh劝萤。
隨后初始化控制輸入 $u$ 向量和測量量z渊涝。
Q,R,H,F和我們之前說的過程噪音協(xié)方差矩陣，測量噪音協(xié)方差矩陣，觀察模型矩陣跨释，狀態(tài)轉(zhuǎn)移矩陣一一對(duì)應(yīng)胸私，請自行回看前面他們?nèi)绾味x并賦值的。這里注意一下F(0,1)在原文中是 $\Delta t$ 鳖谈，我們假設(shè)傳感器每0.1s獲得一次數(shù)據(jù)岁疼，這兒就設(shè)置為0.1了。
初始化完成之后進(jìn)入for循環(huán)進(jìn)行狀態(tài)更新蚯姆，由于我們假設(shè)控制輸入為 $[0, v_{k-1}]^T$ 五续，所以先把時(shí)刻k-1的速度賦值給 $u(1)$ 。每一時(shí)刻傳感器的測量量不同龄恋，我們假設(shè)有個(gè)GetCurrentMeasurement()函數(shù)能幫助我們獲得傳感器當(dāng)前數(shù)值疙驾。
然后就進(jìn)入卡爾曼濾波的標(biāo)準(zhǔn)五步即是predict和update。我們前面的公式只有五步郭毕，代碼沒有冗余也就只有五步它碎。請自行回看和公式變量一一對(duì)應(yīng)。
盡管新手看卡爾曼濾波的公式可能覺得莫名其妙很困難显押，但是代碼如果和公式變量對(duì)應(yīng)上后實(shí)施起來卻非常簡單扳肛，只有五行，你寫不了上當(dāng)寫不了吃虧乘碑，只有五行挖息！
當(dāng)然雖然實(shí)際情況會(huì)復(fù)雜不少，但是這幾行核心的代碼不會(huì)有大的改變兽肤。
請大家好好研讀上面的公式套腹，例子和偽碼，如果還是完全沒Get到點(diǎn)资铡，去看看其他網(wǎng)上教程吧电禀。講卡爾曼濾波的很多，如果還是沒Get到點(diǎn)笤休，我也不管了(手動(dòng)滑稽)尖飞，因?yàn)榻酉聛砭鸵M(jìn)入擴(kuò)展卡爾曼濾波了。

擴(kuò)展卡爾曼濾波

擴(kuò)展卡爾曼濾波是卡爾曼濾波的進(jìn)階版店雅，差別不大政基。擴(kuò)展卡爾曼濾波針對(duì)的是非線性系統(tǒng)。非線性系統(tǒng)更符合現(xiàn)實(shí)生活中我們遇到的實(shí)例闹啦。我們前面講的一維小車的運(yùn)動(dòng)模型腋么，它的狀態(tài)預(yù)測能寫成矩陣形式
$\begin{align} \begin{bmatrix} p_k \\ v_k \end{bmatrix} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} p_{k-1} \\ v_{k-1} \end{bmatrix} + \begin{bmatrix} 0 & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 0 \\ v_{k-1} \end{bmatrix} + \begin{bmatrix} w_1 \\ w_2 \end{bmatrix} \end{align}$
或者說簡寫為
$\mathbf{x}_k = \mathbf{F}_k \mathbf{x}_{k-1} + \mathbf{B}_k \mathbf{u}_k + \mathbf{w}_k$
就單論表達(dá)式，非線性模型的狀態(tài)預(yù)測或者更新亥揖，是無法分離出單獨(dú)的狀態(tài)并寫成矩陣形式的珊擂。什么意思呢圣勒，假設(shè)小車不是勻速運(yùn)動(dòng)，由于某種不可描述的神秘原因摧扇，它當(dāng)前時(shí)刻的速度圣贸，是上一時(shí)刻的速度的正弦值加上一個(gè)控制輸入。我們可以把上面的式子改寫為
$\begin{align} \begin{bmatrix} p_k \\ v_k \end{bmatrix} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} p_{k-1} \\ sin(v_{k-1}) \end{bmatrix} + \begin{bmatrix} 0 & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 0 \\ v_{k-1} \end{bmatrix} + \begin{bmatrix} w_1 \\ w_2 \end{bmatrix} \end{align} \tag{5}$
咦扛稽，這不還是矩陣形式嗎吁峻？確實(shí)是矩陣形式，但如果畫個(gè)圖的話在张， $v_k$ 和 $v_{k-1}$ 的關(guān)系不能再由一個(gè)直線表示出來了用含，而是一條和三角函數(shù)相關(guān)的曲線，因此我們稱它是非線性的帮匾。對(duì)于非線性的公式啄骇，我們再寫成矩陣形式?jīng)]有意義，因?yàn)?br> $\begin{align} \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \end{align}$
不再是狀態(tài)轉(zhuǎn)移矩陣了瘟斜。狀態(tài)轉(zhuǎn)移矩陣需要把之前狀態(tài) $[p_{k-1},v_{k-1}]$ 轉(zhuǎn)移到當(dāng)前狀態(tài) $[p_{k},v_{k}]$ 缸夹，而上面這個(gè)矩陣把 $[p_{k-1},sin(v_{k-1})]$ 轉(zhuǎn)移到了當(dāng)前狀態(tài)。定義都不符合螺句，更不能拿來使用了虽惭。因此對(duì)于非線性運(yùn)動(dòng)模型，我們也不掙扎著要寫成矩陣形式了蛇尚，寫成一個(gè)抽象的函數(shù)就完事兒了芽唇。
$\mathbf{x}_k = \mathbf{f} (\mathbf{x}_{k-1}, \mathbf{u}_{k-1},\mathbf{w}_{k-1})$
對(duì)應(yīng)到卡爾曼濾波中，我們的狀態(tài)預(yù)測(state prediction)公式就應(yīng)該抽象寫為
$\widehat{\mathbf{x}}_{k|k-1} = \mathbf{f} (\widehat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_{k-1})$
$\mathbf{F}_k, \mathbf{B}_k$ 不見了取劫，因?yàn)槲覀兡壳安恢浪麄兪鞘裁礃幼优恪Ｄ敲磫栴}來了，無論如何勇凭，卡爾曼濾波的是需要狀態(tài)轉(zhuǎn)移矩陣來進(jìn)行狀態(tài)估計(jì)的。現(xiàn)在我們沒有咋辦义辕？估計(jì)一個(gè)唄虾标。擴(kuò)展卡爾曼濾波通過狀態(tài)轉(zhuǎn)移式對(duì)狀態(tài)的雅各比矩陣來獲得狀態(tài)轉(zhuǎn)移矩陣。即
$\mathbf{F}_k = \frac{\partial \mathbf{f}}{\partial \mathbf{x}} = \begin{align} \begin{bmatrix} \frac{f_1}{x_1} & \frac{f_1}{x_2} & \cdots & \frac{f_1}{x_n} \\ \frac{f_2}{x_1} & \frac{f_2}{x_2} & \cdots & \frac{f_2}{x_n} \\ \vdots & \vdots & \vdots & \vdots \\ \frac{f_n}{x_1} & \frac{f_n}{x_2} & \cdots & \frac{f_n}{x_n} \end{bmatrix} \end{align} \tag{6}$
對(duì)于我們剛才非線性運(yùn)動(dòng)的一維運(yùn)動(dòng)的小車灌砖，卡爾曼濾波中的狀態(tài)預(yù)測公式完全展開就應(yīng)該寫為
$\begin{align} \begin{bmatrix} \widehat{p}_{k|k-1} \\ \widehat{v}_{k|k-1} \end{bmatrix} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} \widehat{p}_{k-1|k-1} \\ sin(\widehat{v}_{k-1|k-1}) \end{bmatrix} + \begin{bmatrix} 0 & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 0 \\ \widehat{v}_{k-1|k-1} \end{bmatrix} \end{align}$
正如我們剛才提到的璧函，對(duì)于非線性的運(yùn)動(dòng)寫成矩陣形式?jīng)]有太大意義，所以我們寫回一般形式
$\begin{align} & \widehat{p}_{k|k-1} = \widehat{p}_{k-1|k-1} + \Delta t * sin(\widehat{v}_{k-1|k-1}) \\ & \widehat{v}_{k|k-1} = sin(\widehat{v}_{k-1|k-1}) + \widehat{v}_{k-1|k-1} \end{align} \tag{7}$
對(duì)7式按著6式求雅各比矩陣基显，6式中的 $x_1$ 即 $\widehat{p}_{k-1|k-1}$ 蘸吓， $x_2$ 即 $\widehat{v}_{k-1|k-1}$ ， $f_1$ 即7式的上式撩幽， $f_2$ 即7式的下式库继。求完導(dǎo)數(shù)我們可得雅各比矩陣
$\mathbf{F}_k = \frac{\partial \mathbf{f}}{\partial \mathbf{x}} = \begin{align} \begin{bmatrix} 1 & \Delta t * cos(\widehat{v}_{k-1|k-1}) \\ 0 & cos(\widehat{v}_{k-1|k-1})+1\\ \end{bmatrix} \end{align}$
這么一來用于擴(kuò)展卡爾曼濾波 $\; Error \, Covariance \, Prediction$ 這一步的 $\mathbf{F}_k$ 便有了箩艺。此步的 $\mathbf{Q}_k$ 和線性卡爾曼濾波器沒有差別。
類似地宪萄，如果觀察(測量)模型是線性的艺谆，則可以簡寫為 $z_k = \mathbf{H}_k \widehat{\mathbf{x}}_{k|k-1}$ ，在我們一維小車的例子中拜英，它是
$z_k = [1 \ \ \ 0] \begin{bmatrix} \widehat{p}_{k|k-1} \\ \widehat{v}_{k|k-1} \end{bmatrix}$
我們直接觀察就看出 $\mathbf{H}_k = [1,0]$ 静汤。如果觀察模型不是線性的，我們用估計(jì) $F_k$ 同樣的方法估計(jì) $H_k$ 居凶，即非線性情況下我們的觀察模型應(yīng)記為
$z_k = h(\widehat{\mathbf{x}}_{k|k-1})$
那么
$\mathbf{H}_k = \frac{\partial \mathbf{h}}{\partial \mathbf{x}} = \begin{align} \begin{bmatrix} \frac{h_1}{x_1} & \frac{h_1}{x_2} & \cdots & \frac{h_1}{x_n} \\ \frac{h_2}{x_1} & \frac{h_2}{x_2} & \cdots & \frac{h_2}{x_n} \\ \vdots & \vdots & \vdots & \vdots \\ \frac{h_n}{x_1} & \frac{h_n}{x_2} & \cdots & \frac{h_n}{x_n} \end{bmatrix} \end{align} \tag{8}$
我就不再強(qiáng)行舉個(gè)非線性測量模型的例子了虫给，免得看懵。為什么這樣的雅各比矩陣可以作為狀態(tài)轉(zhuǎn)移矩陣和測量矩陣的估計(jì)呢侠碧？又到了讓同學(xué)們自己去看的時(shí)間了(其實(shí)是看過太久自己給忘了)抹估。我們需要記住的是，這種估計(jì)方式把卡爾曼濾波的“完美性質(zhì)”給毀了舆床，卡爾曼濾波被稱為最優(yōu)的無偏濾波器棋蚌，但是擴(kuò)展卡爾曼濾波以及其他形式的用來解決非線性模型的卡爾曼濾波比如UKF(unscented kalman filter)等，都不會(huì)再滿足最優(yōu)和無偏這兩個(gè)性質(zhì)挨队，這兩個(gè)性質(zhì)的意思大家也自己去查谷暮。UKF的估值相較于EKF還是能更接近真實(shí)值，兩個(gè)針對(duì)卡爾曼濾波在非線性模型中的應(yīng)用都很廣盛垦，UKF是因?yàn)楣乐蹈鼫?zhǔn)確湿弦，EKF是因?yàn)閷?shí)施和理論都更簡單，更多人懂腾夯。
綜上颊埃，擴(kuò)展卡爾曼濾波和卡爾曼濾波非常類似，同樣有五步蝶俱，我們把它寫為
$\begin{align} & \mathbf{Predict} \\ & \; State \, Prediction \; : \widehat{\mathbf{x}}_{k|k-1} = \mathbf{f} (\widehat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_{k}) \\ & \; Error \, Covariance \, Prediction : \mathbf{P}_{k|k-1} = \mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^T+ \mathbf{Q}_k \\ &\mathbf{Update} \\ & \; Kalman \, Gain : \mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T+ \mathbf{R}_k )^{-1} \\ & \; State \, Updation : \widehat{\mathbf{x}}_{k|k} = \widehat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \widehat{\mathbf{x}}_{k|k-1}) \\ & \; Error \, Covariance \, Updation : \mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k ) \mathbf{P}_{k|k-1} \end{align}$
其中 $\mathbf{F}_k$ 來源于6式班利， $\mathbf{H}_k$ 來源于8式。

總結(jié)

至此我們非常簡要地解釋了卡爾曼濾波和擴(kuò)展卡爾曼濾波榨呆。我沒有把卡爾曼的推導(dǎo)詳細(xì)地解釋一遍罗标，那樣也太累了，那是專門講解卡爾曼濾波或者狀態(tài)估計(jì)的書籍的任務(wù)积蜻，我還是著重于怎樣應(yīng)用闯割，無論程序再怎么fancy，應(yīng)用就是那五步公式為核心竿拆。這一講連圖片都沒有宙拉，大家可能會(huì)看地比較懵，你可以私下找我討論或者翻閱相關(guān)書籍丙笋。盡管我已經(jīng)盡量簡短地提及理論部分了谢澈，但是我們下一講還得是理論煌贴。因?yàn)橛辛藬U(kuò)展卡爾曼濾波的基礎(chǔ)，我們得把它如何應(yīng)用于SLAM的詳細(xì)闡述出來澳化，下下講才會(huì)進(jìn)入程序崔步。

最后編輯于：2020.01.08 15:42:13

?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請聯(lián)系作者

人面猴
序言：七十年代末，一起剝皮案震驚了整個(gè)濱河市缎谷，隨后出現(xiàn)的幾起案子井濒，更是在濱河造成了極大的恐慌，老刑警劉巖列林，帶你破解...
沈念sama閱讀 211,817評(píng)論 6贊 492
死咒
序言：濱河連續(xù)發(fā)生了三起死亡事件瑞你，死亡現(xiàn)場離奇詭異，居然都是意外死亡希痴，警方通過查閱死者的電腦和手機(jī)者甲，發(fā)現(xiàn)死者居然都...
沈念sama閱讀 90,329評(píng)論 3贊 385
救了他兩次的神仙讓他今天三更去死
文/潘曉璐我一進(jìn)店門，熙熙樓的掌柜王于貴愁眉苦臉地迎上來砌创，“玉大人虏缸，你說我怎么就攤上這事∧凼担” “怎么了刽辙？”我有些...
開封第一講書人閱讀 157,354評(píng)論 0贊 348
道士緝兇錄：失蹤的賣姜人
文/不壞的土叔我叫張陵，是天一觀的道長甲献。經(jīng)常有香客問我宰缤，道長，這世上最難降的妖魔是什么晃洒？我笑而不...
開封第一講書人閱讀 56,498評(píng)論 1贊 284
?港島之戀（遺憾婚禮）
正文為了忘掉前任慨灭，我火速辦了婚禮，結(jié)果婚禮上球及，老公的妹妹穿的比我還像新娘氧骤。我一直安慰自己，他們只是感情好吃引，可當(dāng)我...
茶點(diǎn)故事閱讀 65,600評(píng)論 6贊 386
惡毒庶女頂嫁案：這布局不是一般人想出來的
文/花漫我一把揭開白布筹陵。她就那樣靜靜地躺著，像睡著了一般际歼。火紅的嫁衣襯著肌膚如雪。梳的紋絲不亂的頭發(fā)上姑蓝，一...
開封第一講書人閱讀 49,829評(píng)論 1贊 290
城市分裂傳說
那天鹅心，我揣著相機(jī)與錄音，去河邊找鬼纺荧。笑死旭愧，一個(gè)胖子當(dāng)著我的面吹牛颅筋，可吹牛的內(nèi)容都是我干的。我是一名探鬼主播输枯，決...
沈念sama閱讀 38,979評(píng)論 3贊 408
雙鴛鴦連環(huán)套：你想象不到人心有多黑
文/蒼蘭香墨我猛地睜開眼议泵，長吁一口氣：“原來是場噩夢啊……” “哼！你這毒婦竟也來了桃熄？” 一聲冷哼從身側(cè)響起先口，我...
開封第一講書人閱讀 37,722評(píng)論 0贊 266
萬榮殺人案實(shí)錄
序言：老撾萬榮一對(duì)情侶失蹤，失蹤者是張志新（化名）和其女友劉穎瞳收，沒想到半個(gè)月后碉京，有當(dāng)?shù)厝嗽跇淞掷锇l(fā)現(xiàn)了一具尸體，經(jīng)...
沈念sama閱讀 44,189評(píng)論 1贊 303
?護(hù)林員之死
正文獨(dú)居荒郊野嶺守林人離奇死亡螟深，尸身上長有42處帶血的膿包…… 初始之章·張勛以下內(nèi)容為張勛視角年9月15日...
茶點(diǎn)故事閱讀 36,519評(píng)論 2贊 327
?白月光啟示錄
正文我和宋清朗相戀三年谐宙，在試婚紗的時(shí)候發(fā)現(xiàn)自己被綠了。大學(xué)時(shí)的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片界弧。...
茶點(diǎn)故事閱讀 38,654評(píng)論 1贊 340
活死人
序言：一個(gè)原本活蹦亂跳的男人離奇死亡凡蜻，死狀恐怖，靈堂內(nèi)的尸體忽然破棺而出垢箕，到底是詐尸還是另有隱情划栓，我是刑警寧澤，帶...
沈念sama閱讀 34,329評(píng)論 4贊 330
?日本核電站爆炸內(nèi)幕
正文年R本政府宣布舰讹，位于F島的核電站茅姜，受9級(jí)特大地震影響，放射性物質(zhì)發(fā)生泄漏月匣。R本人自食惡果不足惜钻洒，卻給世界環(huán)境...
茶點(diǎn)故事閱讀 39,940評(píng)論 3贊 313
男人毒藥：我在死后第九天來索命
文/蒙蒙一、第九天我趴在偏房一處隱蔽的房頂上張望锄开。院中可真熱鬧素标，春花似錦、人聲如沸萍悴。這莊子的主人今日做“春日...
開封第一講書人閱讀 30,762評(píng)論 0贊 21
一樁弒父案，背后竟有這般陰謀
文/蒼蘭香墨我抬頭看了看天上的太陽癣诱。三九已至计维，卻和暖如春，著一層夾襖步出監(jiān)牢的瞬間撕予，已是汗流浹背鲫惶。一陣腳步聲響...
開封第一講書人閱讀 31,993評(píng)論 1贊 266
情欲美人皮
我被黑心中介騙來泰國打工，沒想到剛下飛機(jī)就差點(diǎn)兒被人妖公主榨干…… 1. 我叫王不留实抡，地道東北人欠母。一個(gè)月前我還...
沈念sama閱讀 46,382評(píng)論 2贊 360
代替公主和親
正文我出身青樓欢策，卻偏偏與公主長得像，于是被迫代替她去往敵國和親赏淌。傳聞我的和親對(duì)象是個(gè)殘疾皇子踩寇，可洞房花燭夜當(dāng)晚...
茶點(diǎn)故事閱讀 43,543評(píng)論 2贊 349