免费视频淫片aa毛片_日韩高清在线亚洲专区vr_日韩大片免费观看视频播放_亚洲欧美国产精品完整版

此片blog的目的是理解卡爾曼算法的思想和實(shí)際應(yīng)用的物理含義，想法很好，卻只能理解冰山一角，先記下這一角

另本blog參考卡爾曼濾波 -- 從推導(dǎo)到應(yīng)用和徐亦達(dá)卡爾曼推導(dǎo)視頻

首先認(rèn)識(shí)卡爾曼算法在數(shù)學(xué)領(lǐng)域是什么位置：處理線性高斯模型的算法

然后認(rèn)識(shí)卡爾曼算法的思想：

一片綠油油的草地上有一條曲折的小徑,通向一棵大樹.一個(gè)要求被提出:從起點(diǎn)沿著小徑走到樹下.

“很簡(jiǎn)單.” A說,于是他絲毫不差地沿著小徑走到了樹下.

現(xiàn)在，難度被增加了：蒙上眼。

“也不難，我當(dāng)過特種兵。” B說，于是他歪歪扭扭地走到了樹旁。“唉，好久不練，生疏了。”（只憑自己的預(yù)測(cè)能力）

“看我的，我有 DIY的 GPS！” C說，于是他像個(gè)醉漢似地歪歪扭扭的走到了樹旁。“唉，這個(gè) GPS沒做好，漂移太大。”（只依靠外界有很大噪聲的測(cè)量）

“我來試試。”旁邊一也當(dāng)過特種兵的拿過 GPS,蒙上眼，居然沿著小徑很順滑的走到了樹下。（自己能預(yù)測(cè)+測(cè)量結(jié)果的反饋）

“這么厲害！你是什么人?”
“卡爾曼 ! ”
“卡爾曼？！你就是卡爾曼？”眾人大吃一驚。
“我是說這個(gè) GPS卡而慢。”

也就是說，預(yù)測(cè)值有高斯噪聲，測(cè)量值也有高斯噪聲，這2個(gè)噪聲相互獨(dú)立，單獨(dú)的利用任何一個(gè)都不能很好的得到真實(shí)值，所以在2者之間有個(gè)信賴度的問題，應(yīng)該相信誰更多些，這也就是卡爾曼算法的核心，這個(gè)信賴度就是卡爾曼增益，卡爾曼增益通過測(cè)量值和真實(shí)值之間的協(xié)方差最小時(shí)確定的，由此求這個(gè)協(xié)方差偏導(dǎo)為0時(shí)的系數(shù)，這個(gè)系數(shù)就是卡爾曼增益，這樣就能很好的融合預(yù)測(cè)值和測(cè)量值。并且推導(dǎo)卡爾曼增益的時(shí)候，發(fā)現(xiàn)協(xié)方差是可以遞歸的，由此只要?jiǎng)傞_始指定初始協(xié)方差就可以源源不斷的求出卡爾曼增益和新的協(xié)方差，從而不斷的跟新真實(shí)值。

接著是普及數(shù)學(xué)卡爾曼知識(shí)(推導(dǎo)卡爾曼運(yùn)算過程)

最后是實(shí)際應(yīng)用：

1.為了方便表達(dá)，把幾個(gè)公式用矩陣表達(dá)式表達(dá)

2.估計(jì)過程：

,其中x是系統(tǒng)的狀態(tài)向量，大小為n*1列。A為轉(zhuǎn)換矩陣，大小為n*n。u為系統(tǒng)輸入，大小為k*1。B是將輸入轉(zhuǎn)換為狀態(tài)的矩陣，大小為n*k。隨機(jī)變量w為系統(tǒng)噪聲。注意這些矩陣的大小，它們與你實(shí)際編程密切相關(guān)。

(2)測(cè)量值當(dāng)然是由系統(tǒng)狀態(tài)變量映射出來的，方程形式如下：

注意Z是測(cè)量值，大小為m*1(不是n*1，也不是1*1，后面將說明），H也是狀態(tài)變量到測(cè)量的轉(zhuǎn)換矩陣。大小為m*n。隨機(jī)變量v是測(cè)量噪聲。

同時(shí)對(duì)于勻加速模型，假設(shè)下車是勻加速遠(yuǎn)離我們，我們站在原點(diǎn)用超聲波儀器測(cè)量小車離我們的距離。

也就是測(cè)量值直接等于位移。可能又會(huì)問，為什么不直接用測(cè)量值呢？測(cè)量值噪聲太大了，根本不能直接用它來進(jìn)行計(jì)算。試想一個(gè)本來是朝著一個(gè)方向做勻加速運(yùn)動(dòng)的小車，你測(cè)出來的位移確是前后移動(dòng)（噪聲影響），只根據(jù)測(cè)量的結(jié)果，你就以為車子一會(huì)往前開一會(huì)往后開。

對(duì)于狀態(tài)方程中的系統(tǒng)噪聲w和測(cè)量噪聲v，假設(shè)服從如下多元高斯分布，并且w,v是相互獨(dú)立的。其中Q,R為噪聲變量的協(xié)方差矩陣。

看到這里自然要提個(gè)問題，為什么噪聲模型就得服從高斯分布呢？個(gè)人覺得卡爾曼是在線性高斯的環(huán)境下建立的模型，也就是說這個(gè)推導(dǎo)有些公式或者定理只能適用于高斯分布這個(gè)條件，具體的話，個(gè)人只記得如果剛開始協(xié)方差滿足高斯，遞推下去，之后的協(xié)方差都滿足高斯。

      對(duì)于小車勻加速運(yùn)動(dòng)的的模型，假設(shè)系統(tǒng)的噪聲向量只存在速度分量上，且速度噪聲的方差是一個(gè)常量0.01，位移分量上的系統(tǒng)噪聲為0。測(cè)量值只有位移，它的協(xié)方差矩陣大小是1*1，就是測(cè)量噪聲的方差本身。

那么：

Q中，疊加在速度上系統(tǒng)噪聲方差為0.01，位移上的為0，它們間協(xié)方差為0，即噪聲間沒有關(guān)聯(lián)。

(3)噪聲(偏差)是如何處理的。貌似有了這個(gè)模型就能完全估計(jì)系統(tǒng)狀態(tài)了，速度能計(jì)算出，位移也能計(jì)算出(這個(gè)模型下面的圖片有介紹)。那還要卡爾曼干嘛，問題是很多實(shí)際系統(tǒng)復(fù)雜到根本就建不了模。并且，即使你建立了較為準(zhǔn)確的模型，只要你在某一步有誤差，由遞推公式，很可能不斷將你的誤差放大A倍（A就是那個(gè)狀態(tài)轉(zhuǎn)換矩陣），以至于最后得到的估計(jì)結(jié)果完全不能用了。回到最開始的那個(gè)笑話，如果那個(gè)完全憑預(yù)測(cè)的特種兵在某一步偏離了正確的路徑，當(dāng)他站在錯(cuò)誤的路徑上（而他自己以為是正確的）做下一步預(yù)測(cè)時(shí)，肯定走的路徑也會(huì)錯(cuò)了，到最后越走越偏。

既然如此，我們就引進(jìn)反饋。從概率論貝葉斯模型的觀點(diǎn)來看前面預(yù)測(cè)的結(jié)果就是先驗(yàn)，測(cè)量出的結(jié)果就是后驗(yàn)。

是預(yù)測(cè)（先驗(yàn)）值，

是估計(jì)值，

為測(cè)量值的預(yù)測(cè)，在下面的推導(dǎo)中，請(qǐng)注意估計(jì)和預(yù)測(cè)兩者的區(qū)別，不混為一談。由一般的反饋思想我們得到估計(jì)值：

其中，

(5)卡爾曼的核心：求取卡爾曼增益(也就是加權(quán))。這個(gè)里面涉及方差或者協(xié)方差，方差好比誤差的平方和，目的是求和真實(shí)值最小的方差的加權(quán)(個(gè)人理解)。

友情提示：以下看不懂可以結(jié)合上面數(shù)學(xué)推導(dǎo)理解

先看估計(jì)值和真實(shí)值間誤差的協(xié)方差矩陣，提醒一下協(xié)方差矩陣的對(duì)角線元素就是方差，求這個(gè)協(xié)方差矩陣，就是為了利用他的對(duì)角線元素的和計(jì)算得到均方差.

這里請(qǐng)注意ek是向量，它由各個(gè)系統(tǒng)狀態(tài)變量的誤差組成。如勻加速運(yùn)動(dòng)模型里，ek便是由位移誤差和速度誤差，他們組成的協(xié)方差矩陣。表示如下：

其中，Serr代表位移誤差，Verr代表速度誤差,對(duì)角線上就是各自的方差。

把前面得到的估計(jì)值代入這里能夠化簡(jiǎn)得：

同理，能夠得到預(yù)測(cè)值和真實(shí)值之間誤差的協(xié)方差矩陣：

注意到系統(tǒng)狀態(tài)x變量和測(cè)量噪聲之間是相互獨(dú)立的。于是展開（1）式可得：

最后得到：

繼續(xù)展開：

接下來最小均方差開始正式登場(chǎng)了，回憶之前提到的，協(xié)方差矩陣的對(duì)角線元素就是方差。這樣一來，把矩陣P的對(duì)角線元素求和，用字母T來表示這種算子，他的學(xué)名叫矩陣的跡。

最小均方差就是使得上式最小，對(duì)未知量K求導(dǎo),令導(dǎo)函數(shù)等于0，就能找到K的值。

注意這個(gè)計(jì)算式K，轉(zhuǎn)換矩陣H式常數(shù)，測(cè)量噪聲協(xié)方差R也是常數(shù)。因此K的大小將與預(yù)測(cè)值的誤差協(xié)方差有關(guān)。不妨進(jìn)一步假設(shè)，上面式子中的矩陣維數(shù)都是1*1大小的，并假設(shè)H=1，

所以

因此遞推公式中每一步的K就計(jì)算出來了，同時(shí)每一步的估計(jì)協(xié)方差也能計(jì)算出來。但K的公式中好像又多了一個(gè)我們還未曾計(jì)算出來的東西

由于系統(tǒng)狀態(tài)變量和噪聲之間是獨(dú)立，故可以寫成：

由此也得到了

的最初值我們可以設(shè)定；還差

的非初始值，

“死”步驟

再來點(diǎn)lpe的程序分析：那就以氣壓和光流為例

程序就是按上面介紹的思路來寫的：先用物理模型推測(cè)下一刻的狀態(tài)，再用傳感器得到帶有噪聲的位置速度等信息，最后通過算最小協(xié)方差時(shí)的卡爾曼增益，并加權(quán)得到最優(yōu)解。

void BlockLocalPositionEstimator::baroCorrect()
{
	// measure
	Vector<float, n_y_baro> y;
	if (baroMeasure(y) != OK) { return; }
	// subtract baro home alt
	y -= _baroAltHome;
	// baro measurement matrix
	Matrix<float, n_y_baro, n_x> C;
	C.setZero();
	C(Y_baro_z, X_z) = -1; // measured altitude, negative down dir.
	Matrix<float, n_y_baro, n_y_baro> R;
	R.setZero();
	R(0, 0) = _baro_stddev.get() * _baro_stddev.get();
	// residual
	Matrix<float, n_y_baro, n_y_baro> S_I =
		inv<float, n_y_baro>((C * _P * C.transpose()) + R);
	Vector<float, n_y_baro> r = y - (C * _x);
	// fault detection
	float beta = (r.transpose() * (S_I * r))(0, 0);
	if (beta > BETA_TABLE[n_y_baro]) {
		if (_baroFault < FAULT_MINOR) {
			//mavlink_and_console_log_info(&mavlink_log_pub, '[lpe] baro fault, r %5.2f m, beta %5.2f',
			//double(r(0)), double(beta));
			_baroFault = FAULT_MINOR;
		}
	} else if (_baroFault) {
		_baroFault = FAULT_NONE;
		//mavlink_and_console_log_info(&mavlink_log_pub, '[lpe] baro OK');
	}
	// kalman filter correction if no fault
	if (_baroFault < fault_lvl_disable) {
		Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
		Vector<float, n_x> dx = K * r;
		if (!_canEstimateXY) {
			dx(X_x) = 0;
			dx(X_y) = 0;
			dx(X_vx) = 0;
			dx(X_vy) = 0;
		}
		_x += dx;
		_P -= K * C * _P;
	}
}

這段的代碼也很好理解，倒著看。

代碼的目的是矯正高度，怎么矯正？predict()函數(shù)最后面_x += dx;這個(gè)dx就是通過模型推導(dǎo)的，而baroCorrect()函數(shù)_x += dx;這里的dx = K * r;其中K是卡爾曼增益K = _P * C.transpose() * S_I；r是測(cè)量值和預(yù)測(cè)值的誤差，r = y - (C * _x);這里y是測(cè)量值，C是轉(zhuǎn)換矩陣將預(yù)測(cè)矩陣轉(zhuǎn)換成測(cè)量矩陣，_x就是預(yù)測(cè)值。這里的核心就是K的求法，這里涉及了協(xié)方差和一些運(yùn)算，這個(gè)是可以從程序一眼看出來的，具體公式和求法沒研究的很清楚。

相同的，看光流，求法是一樣的，只是測(cè)量矩陣不一樣了，噪聲等“個(gè)性化”東西不一樣，處理過程是一樣的。

如果您覺得此文對(duì)您的發(fā)展有用，請(qǐng)隨意打賞。 
您的鼓勵(lì)將是筆者書寫高質(zhì)量文章的最大動(dòng)力^_^!!