本文由$\mathbf{本}\mathbf{文}\mathbf{由}$ @DavidHan$\mathit{@}\mathit{D}\mathit{a}\mathit{v}\mathit{i}\mathit{d}\mathit{H}\mathit{a}\mathit{n}$出品,轉載請注明出處$\mathbf{出}\mathbf{品}\mathbf{,}\mathbf{轉}\mathbf{載}\mathbf{請}\mathbf{注}\mathbf{明}\mathbf{出}\mathbf{處}$
　　　　文章鏈接：$\mathbf{文}\mathbf{章}\mathbf{鏈}\mathbf{接}\mathbf{：}$http://blog.csdn.net/david_han008/article/details/78573616
　　　　
　　　　

卡爾曼濾波

什麼是卡爾曼濾波？在知乎上見到一些有趣的回答。參考原文

假設，你有兩個感測器，測量同一個資訊，可是他們每次的讀數都不太一樣，怎麼？

取平均

再假設，你知道其中那個貴的感測器應該更加準確一些，便宜的那個應該差一些，那有比取平均更好的辦法嗎？

加權平均

那麼，怎麼加權呢？假設兩個感測器的誤差都符合正態分佈，假設你知道兩個正態分佈的方差，利用這兩個方差，你可以得到一個最優的權重。

接下來重點來了，假設你只有一個感測器，但是你還有一個數學模型，模型可以幫助你算出一個值，但是這個值也不是那麼準，怎麼辦？

把模型算出來的值和感測器測量的值，（就像兩個感測器一樣），取加權平均

OK，最後說明一點：你的數學模型其實只是一個迭代的。也就是說，知道x(k)，才可以求出x(k+1)。問題是x(k)是多少呢？答案是：x(k)就是你上一步卡爾曼濾波得到的值

這就是卡爾曼濾波。

還有第2種，有趣的回答：
假設在軌道上，有一輛小車，在無外力的作用情況下，小車在t時刻的狀態量x(t)只與x(t-1)有關

x(t)=Fx(t−1)$$x(t)=Fx(t-1)$$
t=0時，小車位置服從紅色的正態分佈

我們通過數學模型，預測t=1時刻的小車位置應該是

為什麼正態分佈變得矮胖了，原因很簡單，因為疊加了一層噪聲（或者說不確定性）。
為了避免數學模型帶來的誤差。我們使用鐳射雷達在t=1時刻，進行了測量，下面圖中的藍色的部分

好了，我們現在得到兩個不同的結果，那麼，我們現在進行卡爾曼濾波融合，找到相應的權值。就像下面這個圖所示。紅色和藍色合併為下面的綠色部分。

並且真正的牛逼的之處，在於加權值之後的綠色部分，仍然符合正態分佈，可以作為第二次位置估計的初值，一次迭代下去。

航跡推演

航跡推演涉及的是小車的運動學模型，在這裡涉及到的航跡推演演算法是，已知左右輪的速度，而來的（還有其他航跡推演的方法，例如已知角速度）

假設兩輪間距是L$L$
已知Δsr$\mathrm{\Delta }{s}_r$ , Δsl$\mathrm{\Delta }{s}_l$和Δt$\mathrm{\Delta }t$,在很短的距離內，我們利用已直代曲的思想，將Δs$\mathrm{\Delta }s$近似看成Δd$\mathrm{\Delta }d$

主要的公式：

Δd=Δs=Δsr+Δsl2Δθ=Δsr−ΔslLΔx=Δs∗cos(θ+Δθ/2)Δy=Δs∗sin(θ+Δθ/2)$$\begin{array}{l}\mathrm{\Delta }d=\mathrm{\Delta }s=\frac{\mathrm{\Delta }{s}_r+\mathrm{\Delta }{s}_l}{2}\\ \mathrm{\Delta }\theta =\frac{\mathrm{\Delta }{s}_r-\mathrm{\Delta }{s}_l}{L}\\ \mathrm{\Delta }x=\mathrm{\Delta }s\ast cos(\theta +\mathrm{\Delta }\theta /2)\\ \mathrm{\Delta }y=\mathrm{\Delta }s\ast sin(\theta +\mathrm{\Delta }\theta /2)\end{array}$$

位移量x,y$x,y$
補充：
v=ΔsΔt=vr+vl2$$v=\frac{\mathrm{\Delta }s}{\mathrm{\Delta }t}=\frac{v_r+v_l}{2}$$
w=Δsr−ΔslΔt∗L=vr−vlL$$w=\frac{\mathrm{\Delta }{s}_r-\mathrm{\Delta }{s}_l}{\mathrm{\Delta }t\ast L}=\frac{v_r-v_l}{L}$$
注：這些公式不難，直接在STM32當中計算出來
有了上面的推導公式，不難寫出小車的線速度是多少，角速度是多少，然後在程式碼裡面，只需要對上面的數值進行累加即可。
附 編碼器計算里程：
購買的電機：點選這裡
配套的驅動器：點選這裡
重要結論，電機轉一圈 產生32000QC（也就是32000個脈衝）
計算過程如下：

編碼器500線，通過16:1的減速器，之後兩相輸入驅動器，一個週期有4個上升沿，因此電機轉一圈，產生32000個脈衝。
上面的公式當中Δsr,Δsl$\mathrm{\Delta }{s}_r,\mathrm{\Delta }{s}_l$，可以通過統計Δt$\mathrm{\Delta }t$時間內，編碼器產生的脈衝數，來計算Δsr,Δsl$\mathrm{\Delta }{s}_r,\mathrm{\Delta }{s}_l$的位移量。
假定Δt$\mathrm{\Delta }t$時間內，左輪的脈衝個數是ΔNl

相關推薦

SLAM重要概念，通俗講解（持續更新中）

　　　　本文由本文由 @DavidHan@DavidHan出品,轉載請注明出處出品,轉載請注明出處 　　　　文章鏈接：文章鏈接：http://blog.csdn.net/david_han008/article/details/78573616 　　　　

總結了一些新的面試題，包含答案（持續更新中）

Java根底● 集合類以及集合框架；HashMap與HashTable完成原理，執行緒平安性，hash抵觸及處置演算法；ConcurrentHashMap；- Collection 集合的根介面+Set 繼承類，不能包含重複元素,無序+List 繼承類，可以包含重複元素，

2018年阿里巴巴重要開源專案彙總（持續更新中）

摘要： 雲棲社群特在2018年年末，將阿里巴巴的一些重要的開源專案進行整理，希望對大家有所幫助。 開源展示了人類共同協作，成果分享的魅力，每一次技術發展都是站在巨人的肩膀上，技術諸多創新和發展往往就是基於開源發展起來的，沒有任何一家網路公司可以不使用開源技術，僅靠自身技術而發展起來。阿里巴巴各個團

Android探索之旅（第三十三篇）恩？你想成為Android架構師，我這裡有料呦~~（持續更新中）

筆者認為你若想要成為熟悉及精通Android知識，勢必對於Gradle要求很是熟悉，推薦大家看徐宜生的《Android群英傳·神兵利器》，這本書最突出的就是它對於Gradle講解的非常詳細，讀完這本書之

navicat mysql查資料庫中表名、表數量，欄位名、欄位數量（持續更新中）

1.查資料庫中表數量 （紅色標記的是常用到的重要的表結構資訊表） mysql> use information_schema;Database changedmysql> show tables;+-------------------------------

Android 系統中，那些能大幅提高工作效率的 API 彙總（持續更新中...）

前言 “條條大路通羅馬。”工作中，實現某個需求的方式往往不是唯一的，這些不同實現方式不僅表現在程式碼質量上，還影響著我們的工作效率。就像，在 Android 系統中，總有那麼一些鮮為人知的 API 能夠減少我們很多零碎的工作量。於是，就想憑著一些經

前端學習之路，分享給剛學或想學前端的網友（持續更新中）

　　經常會在群裡或論壇上看到有人問：“學習前端有什麼捷徑？”，一般都是賣油翁式的回答：“無他唯手熟爾”。那麼該如何讓手熟練呢？其實也就是該如何系統的學習前端。在本文中，我會結合自身的經歷，分享一下自己學習前端的過程，期間會穿插引用我過去各個階段所寫的博文。 一

IT(計算機/軟件/互聯網)專業詞匯寶典（持續更新中）

hub point charger 中國 mar asi lose 社區 less 1.Stack Overflow:http://stackoverflow.com/ .一個著名的IT技術的問答站點。全然免費。程序猿必知。2.programmer:程序猿3.e

git的常用語句（持續更新中）

文件 版本 reflog 使用 一個 更改 commit sta add mkdir learngit // 創建文件夾cd learngit //跳轉到指定文件夾pwd //顯示當前文件夾路徑git init //把這個目錄變成可管理的倉庫git add learngi

ASP.NET MVC5+EF6+EasyUI 後臺管理系統（1）-前言與目錄（持續更新中...）

編碼規範 圖標 pri log 任務 ros 部署 基本 form 開發工具：VS2015(2012以上)+SQL2008R2以上數據庫 　　 您可以有償獲取一份最新源碼聯系QQ:729994997 價格 666RMB 升級後界面效果如下： 日程管理 http://

一句話題解（持續更新中）

ron noi rdquo 括號 出現 eps sof 單調性 滾動 8.1 bzoj 4720 noip2016 換教室 floyd預處理+期望（薛定諤的貓） bzoj 4318 OSU！ 三次函數期望值 從一次、二次推得 8.2 bzoj 1076 狀壓+期望D

關於Yii2中的MVC中的視圖總結（持續更新中）

gif pre lis frontend dbo register front reg open 一、首先在控制器中，將處理好的數據發送給前臺： $this->layout = ‘base‘; 這裏填寫視圖的模板文件（可以不寫這行代碼，如果不寫，默認為views/la

java基礎知識匯總（持續更新中....）

方法區 管理 執行 中間 inter print method arch end 1.java四大特性：抽象、繼承、封裝，多態 構造函數： http://blog.csdn.net/qq_33642117/article/details/51909346 　　

第11章—常用註解（持續更新中）

導入 rest class entity auto config cati param tee spring boot 系列學習記錄：http://www.cnblogs.com/jinxiaohang/p/8111057.html 碼雲源碼地址：https://gitee

golang開發常用開源項目工具（持續更新中）

golang開源項目 golang工具 本人在日常golang項目開發中，用到了一些第三方開源庫，大多都是優秀的開源項目，源碼值得認真閱讀研究。在此記錄用到的一些庫:解析配置文件go-ini https://github.com/go-ini/inigolang開發常用開源項目工具（持續更新中）

談談對事件的理解（持續更新中）

use 通過 行為 tlist 理解 scrip 創建 可能 有時 談談對事件的理解： 從前有一家人，我們稱為window家。window他其中有一個兒子叫事件。 -------------------------------------------------------

前端開發工程師從入門到精通學習過程及資源總結（持續更新中）

開發實戰 set dex 從入門到精通 main 知識 uno 基礎入門 易雲 職位名稱：前端開發工程師 學習概要：1、先了解基礎，html/css/JavaScript是基礎。html/css就是做靜態頁面的，javascript是做動態的。 2、學習框架：jQuery（

CS231N作業記錄（持續更新中）

ssi net tail 安裝ipython ipy 工作 href https 準備 參考資料：《 cs231n 課程作業 Assignment 1 》https://blog.csdn.net/zhangxb35/article/details/55223825 一

自動化運維之Ansible概述及Ansible部署|（持續更新中）

nss red hat 優秀 分發 程序 paramik and gho 統一 Ansible概述 由於互聯網的快速發展導致產品更新換代速度逐漸加快，運維人員每天都要進行大量的維護操作，仍舊按照傳統方式進行維護使得工作效率低下。這時，部署自動化運維就可以盡可能安全，高效地完

一個博客萌新的C語言之旅（持續更新中....）

方便 流程 函數聲明 語言 tex 表達 賦值語句 當前 寫在前面 先更新上一次留下的的C語言練習答案，如下： #include <stdio.h> double mj(double r) { return 3.14*r*r; } int main() { i