欧美成人午夜精品久久久,国产?V天堂一区二区三区,欧美精品va在线观看,亚洲一区二区三区免费在线观看,av无码精品一区二区久久,欧美性爱视频不卡一区三区,欧美乱人伦视频在线观看,国产一级牲交高潮

ARTICLE DETAIL

資訊詳情

深耕商務(wù)建站與企業(yè)官網(wǎng)運營的一線實戰(zhàn)洞察。

卡爾曼濾波原理詳解與Python實現(xiàn):傳感器融合與狀態(tài)估計實戰(zhàn)指南

卡爾曼濾波原理詳解與Python實現(xiàn):傳感器融合與狀態(tài)估計實戰(zhàn)指南 開車時手機導(dǎo)航上的定位點是不是經(jīng)常在路口亂跳這不是手機壞了而是GPS信號本身帶噪聲在城區(qū)高樓之間誤差幾米到十幾米都很常見。卡爾曼濾波這種經(jīng)典的狀態(tài)估計算法干的就是從這些帶噪聲的測量里猜出真實狀態(tài)的活。1960年魯?shù)婪颉た柭岢鏊臅r候大概也沒想到半個多世紀(jì)后它依然是自動駕駛、機器人、航空航天領(lǐng)域最常用的傳感器融合算法。這篇文章我不整虛的直接帶你從原理直覺一路走到Python代碼實現(xiàn)把五個核心公式、完整可跑的代碼、調(diào)參經(jīng)驗和發(fā)散避坑一起說透。本文適合這幾類人剛接觸卡爾曼濾波的在校學(xué)生、在機器人和無人機項目里被噪聲折磨的工程師、以及任何想在代碼里用最快速度跑通一個濾波器的人。我會盡量用大白話和具體數(shù)字講原理保證你跟著敲完代碼就能理解每一行在干什么。1. 卡爾曼濾波到底在解決什么問題先說一個最樸素的問題你有一個傳感器讀數(shù)不準(zhǔn)怎么估計真實值最笨的辦法是多測幾次取平均。假設(shè)你用一個溫度計測恒溫箱溫度每次讀數(shù)都不一樣取10次的平均值確實比單次讀數(shù)穩(wěn)。但這個辦法有個明顯的缺點如果溫度本身在緩慢變化你取平均得到的是過去一段時間的平均溫度而不是當(dāng)前溫度滑窗越大滯后越嚴(yán)重。而且你沒有利用溫度隨時間的變化規(guī)律這個信息——實際上我們通常知道這個規(guī)律比如熱慣量導(dǎo)致溫度不可能瞬間跳變??柭鼮V波的思路完全不一樣它同時使用兩個信息來源一個是系統(tǒng)模型對下一時刻狀態(tài)的預(yù)測另一個是傳感器對當(dāng)前狀態(tài)的觀測然后按照兩者的可信度動態(tài)加權(quán)。這個可信度不是拍腦袋定的而是隨著每次遞歸更新自動調(diào)整的。這就是它和滑動平均、低通濾波器最本質(zhì)的區(qū)別。1.1 用溫度測量的例子說清濾波的本質(zhì)假設(shè)恒溫箱的真實溫度是23.0°C溫度計的觀測噪聲方差是1.0也就是標(biāo)準(zhǔn)差1°C。單次讀數(shù)可能在22到24之間浮動。如果用卡爾曼濾波我們首先建立一個非常簡單的系統(tǒng)模型下一時刻的溫度 ≈ 當(dāng)前溫度 過程噪聲。過程噪聲代表你沒有建模的因素比如開關(guān)門帶進來的氣流、加熱器自身的波動假設(shè)它方差很小比如0.01。這樣就有了兩個不完美的信息來源模型預(yù)測基于上一時刻估計值外推好處是穩(wěn)定壞處是沒有觀測修正會越來越偏。傳感器觀測直接反映當(dāng)前狀態(tài)好處是真實壞處是每個點都帶噪聲。卡爾曼濾波每一輪做的事情就一句話用模型預(yù)測得到先驗估計再用觀測修正得到后驗估計。至于修正多少由一個叫卡爾曼增益的系數(shù)決定。這個增益不是固定的而是每一幀動態(tài)算出來的。1.2 為什么滑動平均和低通濾波不是最優(yōu)解拿滑動平均來說它隱含的假設(shè)是所有歷史數(shù)據(jù)的權(quán)重相等這在系統(tǒng)狀態(tài)穩(wěn)定時沒問題但一旦狀態(tài)發(fā)生變化比如目標(biāo)突然加速滑動平均會反應(yīng)遲鈍因為舊數(shù)據(jù)還在起著同樣的權(quán)重。低通濾波雖然可以做到指數(shù)衰減權(quán)重但系數(shù)的選擇基本靠經(jīng)驗和試錯而且無法顯式處理我模型預(yù)測得準(zhǔn)不準(zhǔn)傳感器這陣子可靠不可靠這類問題??柭鼮V波相比它們的核心優(yōu)勢有三個自適應(yīng)權(quán)重??柭鲆鍷會隨著協(xié)方差的收斂自動變化。剛開始估計不確定它更相信觀測隨著估計越來越穩(wěn)它逐漸轉(zhuǎn)向相信模型預(yù)測。遞歸在線處理。你不需要存儲一段歷史數(shù)據(jù)每一幀只需要保留上一幀的狀態(tài)向量和協(xié)方差矩陣計算量固定非常適合嵌入式實時系統(tǒng)。真正利用了系統(tǒng)動力學(xué)。模型告訴濾波器系統(tǒng)會按照某個規(guī)律演化濾波器也同時評估這個規(guī)律本身的可信度。當(dāng)然這些優(yōu)勢是有前提的系統(tǒng)模型近似線性過程噪聲和觀測噪聲近似高斯分布。在這兩個前提滿足時卡爾曼濾波在線性高斯意義下就是均方誤差最小的貝葉斯遞歸估計器。如果系統(tǒng)非線性太強后面會提到擴展卡爾曼和無跡卡爾曼。1.3 卡爾曼濾波的適用邊界與選型判斷不是所有問題都適合卡爾曼濾波。我常跟人講一個篩選邏輯先問自己三個問題。第一你是否有一個可以寫成狀態(tài)遞推的模型比如下一時刻位置 當(dāng)前速度 × Δt 當(dāng)前位置這就夠了。如果沒有可能更適合滑動平均或直接回歸。第二你的傳感器噪聲是不是近似高斯白噪聲偏差bias和粗差尖峰不屬于高斯白噪聲前者需要標(biāo)定或擴展?fàn)顟B(tài)估計后者需要異常值剔除。實際傳感器大多數(shù)同時存在兩者。第三你對實時性有沒有要求卡爾曼濾波的優(yōu)勢之一是計算量小非常適合實時系統(tǒng)但如果你的數(shù)據(jù)是離線批處理、精度要求極高粒子濾波等非線性方法可能更好。這個問題定位清楚之后我們再進入正題看看怎么把一個物理問題寫成卡爾曼濾波要求的矩陣形式。2. 寫方程之前先把模型搭起來卡爾曼濾波的核心輸入不是代碼而是兩個方程狀態(tài)轉(zhuǎn)移方程和觀測方程。我第一次接觸這東西時急著看公式和代碼結(jié)果發(fā)現(xiàn)代碼跑出來完全不對回頭補了線性系統(tǒng)的知識才明白問題在建模上。所以這一章我必須重點講。2.1 狀態(tài)向量怎么選先說狀態(tài)向量。以一輛直線行駛的汽車為例如果我們只知道位置x模型就只能寫成位置下一幀 位置上一幀 噪聲這個模型沒有速度信息任何速度變化都被歸進了過程噪聲濾波效果會很差。一個好的做法是讓狀態(tài)向量包含決定系統(tǒng)演化的所有關(guān)鍵量。對于勻速直線運動至少是x_k [位置, 速度]^T如果你希望模型允許勻加速可以再加一個加速度維度x_k [位置, 速度, 加速度]^T這里不是狀態(tài)越多越好。狀態(tài)維度增加計算量和協(xié)方差矩陣的復(fù)雜度都會上升而且加速度維度如果本身很不穩(wěn)定模型可能反而不準(zhǔn)。工程里常見的選擇是能用二維位置速度解決的就別堆三維。模型越簡單需要調(diào)的參數(shù)越少發(fā)散的概率越低。2.2 狀態(tài)轉(zhuǎn)移矩陣A和控制輸入B有了狀態(tài)向量下一步寫狀態(tài)轉(zhuǎn)移方程。勻速直線運動的離散化結(jié)果是p_{k1} p_k v_k Δt v_{k1} v_k寫成矩陣形式x_{k1} [[1, Δt], [0, 1]] x_k w_k這里A矩陣是A [[1, Δt], [0, 1]]它的作用就是把上一幀狀態(tài)按物理規(guī)律外推到當(dāng)前幀。如果模型里還有外部確定的輸入比如剎車帶來的明確加速度可以寫成 B u_k 形式。但對于大多數(shù)狀態(tài)估計問題外部輸入要么沒有要么已經(jīng)被當(dāng)作隨機過程處理所以不寫B(tài)項也不會影響主流程。在Python里用numpy定義A np.array([[1.0, dt], [0.0, 1.0]])就是這個簡單的2x2矩陣承載了勻速外推這層全部語義。dt是多長時間由你的采樣周期決定。這個值直接決定了位置預(yù)測對速度的依賴度很重要別寫錯。2.3 觀測方程和觀測噪聲協(xié)方差R觀測方程描述傳感器測到的到底是什么。GPS測的是位置測不了速度所以觀測矩陣H要把速度分量抽出去H [[1, 0]]公式是z_k H x_k v_kv_k的協(xié)方差R就是你傳感器讀數(shù)的噪聲方差。這個值怎么確定最實用的方法是找一個靜止場景采集幾十到幾百個觀測值直接算方差。比如把GPS天線放在樓頂固定點采200個位置點計算東向和北向坐標(biāo)的方差就能得到R矩陣的對角元素。常有同學(xué)直接把傳感器說明書上的誤差指標(biāo)當(dāng)R用這在論文里可以但在工程里會翻車——手冊給的是理想環(huán)境指標(biāo)實際受多徑效應(yīng)、溫度漂移的影響遠大于手冊值。所以我都會建議R要么用實測數(shù)據(jù)算出來要么初始給一個偏大的值再調(diào)。2.4 過程噪聲Q的物理含義與矩陣構(gòu)造Q表示你對模型本身的信任程度。一個看似很奇怪但很關(guān)鍵的問題如果系統(tǒng)完全是勻速直線運動Q是不是應(yīng)該取0理論上是的但現(xiàn)實中沒有完全勻速的物體——車輪打滑、空氣阻力、路面傾斜都會帶來未建模的加速度。Q就是為所有你沒寫進A矩陣的物理效應(yīng)留的余地。如果Q設(shè)得太小濾波器會過度相信模型一旦目標(biāo)稍微機動一點估計就追不上了。如果Q設(shè)得太大濾波器又會被觀測噪聲帶著走平滑效果全無。在勻加速機動建模中一個常用的構(gòu)造方式是把加速度當(dāng)作白噪聲推導(dǎo)出的過程噪聲協(xié)方差矩陣是Q σ_a2 × [[Δt?/4, Δt3/2], [Δt3/2, Δt2]]這里的σ_a2是加速度的功率譜密度反映機動強度。你可能好奇為什么位置和速度的協(xié)方差項有一點五次方和平方的關(guān)系——因為位置受加速度影響是Δt2量級速度受加速度影響是Δt量級二者必然在時間累積上有相關(guān)性。直接記住這個公式也可以但理解來源對調(diào)參很有幫助。如果你只有一個標(biāo)量狀態(tài)比如溫度Q就退化成一個標(biāo)量比如0.01含義是每一步模型預(yù)測的方差是0.01。到這里建模階段完成。你會發(fā)現(xiàn)并沒有引入什么高深數(shù)學(xué)只是把一個物理過程老老實實地寫成了矩陣遞推。接下來才是那個讓人頭疼的部分五個公式到底是怎么來的。3. 卡爾曼濾波五大核心公式的直觀推導(dǎo)卡爾曼濾波的五個公式看起來像天書但本質(zhì)上就是兩件事的數(shù)學(xué)化先按模型預(yù)測再用觀測修正。這一章我用最直白的方式拆一遍并且給一個手算數(shù)值例子你看完會發(fā)現(xiàn)它不過是一套帶權(quán)重的遞推平均。3.1 預(yù)測步驟從上一幀推先驗假設(shè)上一幀的最優(yōu)估計是x?_{k-1}協(xié)方差是P_{k-1}。第一步用狀態(tài)轉(zhuǎn)移矩陣外推先驗估計x?_k^- A x?_{k-1}然后更新先驗協(xié)方差P_k^- A P_{k-1} A^T Q這里為什么是A乘P再乘A轉(zhuǎn)置而不是直接乘A因為協(xié)方差的傳播遵循線性變換法則如果 y A x那么 cov(y) A cov(x) A^T??梢园阉斫鉃檎`差傳播A會把狀態(tài)的不確定性拉伸和旋轉(zhuǎn)轉(zhuǎn)置的A是為了保持協(xié)方差矩陣的對稱性。這個兩邊乘A的寫法在工程里非常常見也是很多人在實現(xiàn)時最容易漏掉的地方。P矩陣的對角線元素多大了濾波器就有多沒底。初始幀P0設(shè)得大濾波器就知道自己啥都不確定會放開了信觀測隨著持續(xù)更新P逐漸收斂到一個較小區(qū)間濾波器也開始更信任自己的預(yù)測。3.2 卡爾曼增益K的核心地位接下來是重頭戲卡爾曼增益K P_k^- H^T (H P_k^- H^T R)^{-1}這個公式看著嚇人其實可以逐項拆開理解。H P^- H^T 表示如果把先驗估計投影到觀測空間它的不確定度是多少R是觀測噪聲協(xié)方差。兩者的和是總不確定度。K就是不確定度中來自模型預(yù)測的那一部分占比。如果P^-遠大于R說明你模型預(yù)測特別沒底而觀測很可靠此時K接近1濾波結(jié)果幾乎等于觀測值。反過來如果P^-遠小于R說明你模型預(yù)測已經(jīng)很有把握觀測反而全是噪聲此時K接近0濾波結(jié)果幾乎等于預(yù)測值。所以K在數(shù)值上一定落在0到1之間標(biāo)量情況它就是一個動態(tài)的信任權(quán)重。第一次見到這個權(quán)重是自動算出來、不用手工設(shè)置時我是真的覺得這套理論很優(yōu)雅。3.3 更新公式貝葉斯視角下的數(shù)據(jù)融合有了K之后更新分三步x?_k x?_k^- K (z_k - H x?_k^-)P_k (I - K H) P_k^-第一個公式里 (z_k - H x?_k^-) 叫新息innovation它衡量觀測和預(yù)測之間的差距。如果完全沒有差距說明預(yù)測已經(jīng)完美不需要修正如果差距很大說明預(yù)測嚴(yán)重偏離實際需要把估計往觀測方向拉。拉多少乘以K。第二個公式是協(xié)方差收縮每做一次測量更新P_k只會比P_k^-小或持平因為觀測總是攜帶信息的除非K0。這反映了不確定性的減少也讓濾波器在長期運行中保持馴服。值得一提的細(xì)節(jié)是P_k (I - K H) P_k^- 在數(shù)學(xué)上沒問題但在浮點計算中可能因為減到的實際數(shù)值太小而打破對稱正定性所以我工程上基本改用Joseph形式P_k (I - K H) P_k^- (I - K H)^T K R K^T這個式子數(shù)值上穩(wěn)定得多后面代碼里我會給出用法。3.4 手算一輪迭代把數(shù)字釘進腦袋里公式說多了容易暈我們拿一個標(biāo)量例子完整算一輪。一維常量模型A1H1Q0.01R0.1。初始狀態(tài) x?00P01。第一次觀測值是 z10.5。預(yù)測x?1^- 1 × 0 0P1^- 1 × 1 × 1 0.01 1.01增益K1 1.01 / (1.01 0.1) 0.9099更新x?1 0 0.9099 × (0.5 - 0) 0.4549P1 (1 - 0.9099) × 1.01 0.0909發(fā)現(xiàn)沒有第一輪因為P0設(shè)得大濾波器認(rèn)為我的預(yù)測完全不可信而你觀測噪聲只有0.1比你可靠多了所以K接近0.91估計結(jié)果被觀測牢牢拉住。接著看第二輪。第二次觀測 z20.7。先預(yù)測x?2^- 0.4549P2^- 0.0909 0.01 0.1009增益K2 0.1009 / (0.1009 0.1) 0.5023更新x?2 0.4549 0.5023 × (0.7 - 0.4549) 0.5780P2 (1 - 0.5023) × 0.1009 0.0502這輪的K明顯比第一輪小了。原因很簡單經(jīng)過一輪更新濾波器的預(yù)測不確定性已經(jīng)降到和觀測噪聲差不多同量級所以它不再那么迷信觀測而是把預(yù)測和觀測按約一半一半的比例融合。這個過程會持續(xù)P最終會收斂到一個穩(wěn)態(tài)值K也趨于穩(wěn)定系統(tǒng)進入平衡工作狀態(tài)。理解了這一個標(biāo)量例子基本上就理解卡爾曼濾波的全部本質(zhì)了。4. Python實現(xiàn)與仿真驗證現(xiàn)在進入正題上代碼。我這里給兩個完整例子一個一維常量估計一個二維位置速度跟蹤。每個都能直接復(fù)制運行你只需要裝好numpy和matplotlib。4.1 通用一維卡爾曼濾波器實現(xiàn)先寫一個最簡潔的標(biāo)量版本它雖然只有幾行但已經(jīng)把五個公式全部包含import numpy as np import matplotlib.pyplot as plt from math import sqrt def kalman_filter_1d(meas, A1.0, H1.0, Q0.01, R1.0, x00.0, P01.0): x x0 P P0 est [] cov [] for z in meas: # 預(yù)測 x_pred A * x P_pred A * P * A Q # 更新 K P_pred * H / (H * P_pred * H R) x x_pred K * (z - H * x_pred) P (1 - K * H) * P_pred est.append(x) cov.append(P) return np.array(est), np.array(cov)這段代碼的A、H、Q、R都是標(biāo)量所以看不出矩陣運算的麻煩。它的意義在于讓初學(xué)者一眼看清預(yù)測兩步加更新三步的遞歸結(jié)構(gòu)。函數(shù)返回的是每一幀的估計值和對應(yīng)的協(xié)方差序列協(xié)方差序列可以拿來觀察收斂過程。4.2 一維場景恒溫箱溫度估計用這個函數(shù)來做恒溫箱溫度估計。真實溫度設(shè)為23.0°C觀測噪聲方差1.0過程噪聲方差0.01。初始估計故意設(shè)成0P0設(shè)成1看看濾波器能不能在幾步之內(nèi)從完全錯誤的初值追上來。np.random.seed(42) N 100 true_val 23.0 R 1.0 # 模擬100次溫度計讀數(shù) meas np.random.normal(loctrue_val, scalesqrt(R), sizeN) # 卡爾曼濾波 est, cov kalman_filter_1d(meas, Q0.01, RR, x00.0, P01.0) plt.figure(figsize(10, 4)) plt.plot(meas, alpha0.5, linewidth1, label溫度計觀測) plt.plot(est, linewidth2, label卡爾曼估計) plt.axhline(true_val, colorred, linestyle--, linewidth1, label真實溫度) plt.legend() plt.xlabel(采樣幀) plt.ylabel(溫度 (°C)) plt.title(一維卡爾曼濾波恒溫箱溫度估計) plt.show() print(穩(wěn)態(tài)協(xié)方差約為:, cov[-1])這段代碼的關(guān)鍵點在于觀測值的野跳非常明顯但估計曲線幾乎只在23°C附近輕微浮動。前幾幀從0快速逼近23的過程對應(yīng)的是協(xié)方差收縮和增益K從0.9向0.09快速下降的過程。跑完你會發(fā)現(xiàn)一個有趣的對比單純看單個觀測樣本最大值可能沖到25°C最小值可能掉到21°C但濾波輸出基本在22.9到23.1之間。這說明卡爾曼濾波確實把觀測噪聲抑制掉了一大部分代價是響應(yīng)略微平滑。對于恒溫箱這種緩慢變化的對象這種平滑正是我們想要的。4.3 二維場景位置-速度跟蹤完整實現(xiàn)再升一個維度寫位置-速度聯(lián)合估計。目標(biāo)是讓濾波器在只觀測位置的情況下同時估計出速度。這個場景非常典型本質(zhì)上就是GPS測位但不測速而我們需要知道速度來完成導(dǎo)航。dt 0.1 N 300 # 真實軌跡前150幀勻加速后150幀減速模擬機動 true_pos np.zeros(N) true_vel np.zeros(N) for i in range(N - 1): if i 150: a 0.5 else: a -0.3 true_vel[i 1] true_vel[i] a * dt true_pos[i 1] true_pos[i] true_vel[i 1] * dt # 觀測只有位置標(biāo)準(zhǔn)差 sqrt(0.25)0.5m r 0.25 meas true_pos np.random.normal(0, sqrt(r), N) # 卡爾曼濾波 q 0.2 A np.array([[1.0, dt], [0.0, 1.0]]) H np.array([[1.0, 0.0]]) Q np.array([[q * dt**4 / 4, q * dt**3 / 2], [q * dt**3 / 2, q * dt**2]]) R np.array([[r]]) x np.array([0.0, 0.0]) # 初始位置、速度 P np.eye(2) * 10.0 # 初始協(xié)方差給大一點 est_pos np.zeros(N) est_vel np.zeros(N) for i, z in enumerate(meas): # 預(yù)測 x_pred A x P_pred A P A.T Q # 更新 S H P_pred H.T R K P_pred H.T np.linalg.inv(S) innovation z - (H x_pred).item() x x_pred K.ravel() * innovation P (np.eye(2) - K H) P_pred est_pos[i] x[0] est_vel[i] x[1] # 評估 rmse sqrt(np.mean((est_pos - true_pos)**2)) print(f位置估計RMSE: {rmse:.4f} m) plt.figure(figsize(10, 4)) plt.plot(true_pos, linewidth2, label真實位置) plt.plot(meas, alpha0.4, linewidth1, label帶噪觀測) plt.plot(est_pos, linewidth1.5, linestyle--, label卡爾曼估計位置) plt.legend() plt.xlabel(采樣幀) plt.ylabel(位置 (m)) plt.title(二維卡爾曼濾波位置-速度聯(lián)合估計) plt.show()這里有幾個實現(xiàn)細(xì)節(jié)需要注意。首先是K的維度。K是2x1的矩陣innovation是標(biāo)量所以用K.ravel()把它拉平再乘標(biāo)量得到2維修正向量。如果你寫成K z這種形式記得把z包成1x1矩陣或者像我這樣直接處理為標(biāo)量更直觀。其次Q矩陣的構(gòu)造用了白噪聲加速度模型。q0.2意味著加速度不確定性在一個周期內(nèi)對位置引入約0.2×Δt?/4的方差這個數(shù)值比觀測噪聲的0.25略小表示我們愿意讓濾波器相信目標(biāo)在大部分時間里運動是規(guī)律的。如果q太小那后半段減速機動的時候估計會滯后很多。再看RMSE的結(jié)果。在這個參數(shù)下觀測噪聲標(biāo)準(zhǔn)差是0.5m濾波后的位置RMSE通常在0.2到0.3m之間改善明顯。速度估計的均方誤差也相當(dāng)?shù)碗m然沒有任何傳感器直接測速度但濾波器通過位置的差分加模型約束把速度推出來了。4.4 輸出結(jié)果怎么看收斂、平滑與跟蹤滯后跑完代碼重點看三件事。第一是估計曲線和真實位置的重合度。如果估計曲線在機動段第150幀附近明顯落后真值說明q太小濾波器過于信任勻速模型。如果估計曲線瘋狂抖動和觀測曲線幾乎重合說明q太大濾波基本失去了平滑能力。第二是初始幾幀的快速修正。二維代碼里P0設(shè)成10倍單位矩陣目的就是讓濾波器在最初的幾幀內(nèi)快速把誤差消化掉。你可以試試把P0改成0.01再把初始位置從0開始但真值在50m處你會發(fā)現(xiàn)濾波器會長時間貼著錯的初值跑不動。這就是初始協(xié)方差太小導(dǎo)致早期無法修正的經(jīng)典現(xiàn)象。第三是速度估計的滯后。位置跟蹤的滯后看起來不明顯但速度估計對機動反饋慢很多。如果你把q調(diào)大速度響應(yīng)會變快但噪聲同時變大調(diào)小則反過來。這就是卡爾曼濾波里最經(jīng)典的響應(yīng)速度 vs 平滑程度的權(quán)衡本質(zhì)上是Q和R的博弈。5. 參數(shù)調(diào)優(yōu)、發(fā)散問題與工程中的坑代碼能跑只是起點。真正讓卡爾曼濾波好用的是參數(shù)調(diào)試和異常處理。這一章全是我的實際經(jīng)驗每一個坑都踩過。5.1 P0、Q、R三個矩陣各自的調(diào)參手感先說P0。它的影響主要在最初幾十幀。工程上我的習(xí)慣是對完全沒把握的初始狀態(tài)P0給到對角線10到100級別讓濾波器先犯錯再快速修正。如果你有前幾個靜態(tài)幀數(shù)據(jù)也可以用第一個觀測值的方差來初始化這樣更穩(wěn)。R的調(diào)法最簡單不要拍腦袋去測。把傳感器固定不動讀取200個靜態(tài)樣本算方差那就是R的下界。如果實際場景中傳感器噪聲會因環(huán)境變化而增大我習(xí)慣把R設(shè)置成靜態(tài)方差的兩倍留點安全余量。R設(shè)小了濾波器會過度信任觀測在傳感器漂移時會掛得很慘。Q的調(diào)法最難因為它本質(zhì)上是你所有未建模誤差的匯總。我調(diào)Q時一般遵循這個流程先給一個偏小的Q比如0.01或0.1看觀測殘差是否顯著大于理論值。如果殘差大說明模型欠配Q偏小如果殘差正常但估計曲線抖動說明Q偏大噪聲被放進來了。反復(fù)迭代幾次直到殘差統(tǒng)計和理論吻合。這里給一張我常用的速查表參數(shù)偏離情況典型表現(xiàn)調(diào)整方向Q相對R過小估計曲線過于平滑機動跟不住殘差系統(tǒng)性偏大增大QQ相對R過大估計曲線抖動嚴(yán)重幾乎跟著觀測走減小QR設(shè)得過小濾波器過于信任觀測傳感器一抖就跟著抖增大RR設(shè)得過大濾波器過于信任模型突發(fā)測量變化被忽略減小RP0過小且初值錯誤初始跟蹤極慢長時間飛不到真值附近增大P0別小看這張表我見過太多人在真實項目里被Q/R比值要多少折磨。沒有萬能參數(shù)只有通過殘差分析才能確定。5.2 濾波發(fā)散的具體表現(xiàn)與背后原因發(fā)散是卡爾曼濾波最讓人崩潰的問題估計結(jié)果飛上天完全脫離真值。本質(zhì)原因只有一個——模型和觀測的統(tǒng)計特性假設(shè)與實際不符而這個不匹配導(dǎo)致協(xié)方差P持續(xù)更新錯誤。最常見的發(fā)散場景有三個。第一個是Q過小目標(biāo)機動。目標(biāo)在轉(zhuǎn)彎或變道但模型假設(shè)勻速。每一幀的新息都是正的觀測在預(yù)測方向之前濾波器雖然會修正但每次修正量都被小Q壓住速度估計始終追不上位置誤差越來越大。表現(xiàn)就是開車時導(dǎo)航位置一直在后面跟著。第二個是R被嚴(yán)重低估。你給了傳感器一個比實際小很多的R濾波器就會非常自信地接受觀測噪聲等效于把噪聲整個放進了估計。表現(xiàn)是估計軌跡毛刺極多且協(xié)方差矩陣收斂到一個很小但完全不真實的數(shù)值。發(fā)散時P矩陣甚至可能違背直覺地不斷變小這是最隱蔽的。第三個是數(shù)值問題引發(fā)的協(xié)方差非正定。比如使用了P (I - KH)P^-這個簡化形式并長時間循環(huán)浮點誤差可能讓協(xié)方差的對角線出現(xiàn)負(fù)值或非對稱。解決方法是改用Joseph形式并且定期強制對稱P (np.eye(2) - K H) P_pred (np.eye(2) - K H).T K R K.T P (P P.T) / 2再給一個診斷小技巧保存每一幀的新息innovation序列計算它的均值和方差和理論值 HPH^TR 做對比。如果均值明顯偏離零或者實際方差遠大于理論方差基本能鎖定模型的失配問題。5.3 異常值處理卡方門限與自適應(yīng)R傳感器偶爾會冒出離譜的粗差——GPS在隧道里信號跳變、毫米波雷達被強反射干擾都是真實場景的常態(tài)。如果讓這種觀測正常參與濾波結(jié)果就是位置瞬間被拉飛要好幾個周期才能收回來。我的做法是在更新前加一個粗差檢測門。核心思想是如果新息和它的理論協(xié)方差不相容就認(rèn)為這個觀測是異常的直接丟棄或用另一個放大的R更新。具體方法是用馬氏距離。新息 e z - H x?^-其理論協(xié)方差為 S H P^- H^T R。計算門限量g e^T S^{-1} e如果系統(tǒng)是真高斯分布這個g應(yīng)該服從自由度等于觀測維度的卡方分布。比如觀測維度是1時95%置信的門限是3.8499%是6.63。如果g超過門限就說明觀測與模型預(yù)測不一致的概率很大。簡單的工程實現(xiàn)S H P_pred H.T R e z - (H x_pred).item() g e * e / S.item() if g 6.63: # 卡方0.99門限 # 正常更新 K P_pred H.T np.linalg.inv(S) x x_pred K.ravel() * e else: # 丟棄該觀測純預(yù)測 x x_pred這個方法簡單又好用。不過要注意極端情況如果目標(biāo)真的在劇烈機動并且你的Q設(shè)得很小那么每一幀的新息都會很大卡方門限會把所有觀測全部拒掉濾波器就完全癱了。所以門限檢測要和合理的Q配套使用同時做加速度突變的檢測二者結(jié)合才安全。5.4 數(shù)值穩(wěn)定性求逆用Cholesky還是inv寫代碼時很多同學(xué)喜歡直接用np.linalg.inv(S)低維度下很少出問題但工程上我不建議這么干原因有兩個。一是效率。一維二維問題無所謂但狀態(tài)維度上了10S的維度也上去了inv的計算量是O(n3)而卡爾曼濾波是實時系統(tǒng)能省則省。二是數(shù)值穩(wěn)定性。S通常是對稱正定矩陣用inv求逆可能由于舍入誤差引入不對稱性。我習(xí)慣用Cholesky分解或者numpy.linalg.solve# 用solve代替invK P_pred H.T inv(S) # 等價于解線性方程組 S K.T H P_pred.T K_T np.linalg.solve(S, H P_pred.T) K K_T.T這個寫法的數(shù)值穩(wěn)定性比直接inv好一些而且代碼更貼近矩陣方程的數(shù)學(xué)本質(zhì)。如果你的狀態(tài)量是標(biāo)量那就無所謂了直接除法就行。除了求逆另一個數(shù)值坑是協(xié)方差矩陣在長期運行后變得不對稱甚至非正定。我上面的代碼里已經(jīng)加了強制對稱處理這在小規(guī)模仿真中看不出來但在長時間運行的嵌入式系統(tǒng)里作用巨大。5.5 擴展卡爾曼濾波EKF和無跡卡爾曼濾波UKF的方向如果你的系統(tǒng)模型不是線性的經(jīng)典的卡爾曼濾波就不適用了。典型的非線性場景有兩類一類是傳感器模型非線性比如雷達給出的是距離和方位角而狀態(tài)是直角坐標(biāo)另一類是狀態(tài)轉(zhuǎn)移本身非線性比如無人機姿態(tài)動力學(xué)。處理非線性的主流路數(shù)有兩種擴展卡爾曼濾波EKF是對非線性方程在估計點附近做一階泰勒展開本質(zhì)上是每個時刻都重新線性化。優(yōu)點是實現(xiàn)簡單、計算量小缺點是一階近似在強非線性下精度差而且需要推導(dǎo)雅可比矩陣有點繁瑣。無跡卡爾曼濾波UKF不線性化函數(shù)而是生成一批稱為sigma點的采樣點讓它們通過非線性函數(shù)再從變換后的點中重構(gòu)均值和協(xié)方差。它在精度和實現(xiàn)難度之間是很均衡的選擇不需要推導(dǎo)雅可比矩陣非線性較強時也比EKF穩(wěn)定。但如果你的問題真的極度非線性、多峰值比如從地圖匹配中做全局定位那應(yīng)該考慮粒子濾波??柭易褰鉀Q的是高斯單峰假設(shè)下的最優(yōu)問題在多模態(tài)場景下會整體失效。這一節(jié)不是簡單的知識延伸。我的建議是一定要先理解線性卡爾曼再上非線性否則你連EKF哪里近似都不知道出問題更不會排查。很多同學(xué)直接上手EKF結(jié)果完全跑偏回頭發(fā)現(xiàn)是線性化點選錯了后悔不迭。最后分享一個我在實際項目里反復(fù)驗證過的習(xí)慣拿到新系統(tǒng)先用一個很小的數(shù)據(jù)集把濾波器跑通把真值、觀測、估計三條曲線和殘差圖畫出來確認(rèn)邏輯正確再談?wù){(diào)參。不放圖就跑仿真很難發(fā)現(xiàn)Q/R方向拿沒拿對。另一個習(xí)慣是把濾波器封裝成獨立類方便在不同傳感器之間切換和對比不同變體。卡爾曼濾波表面上是五個公式真正的難點永遠是建模和噪聲刻畫這兩樣做好了代碼只是幾行公式翻譯而已。
返回列表
PREV
查看更多資訊
NEXT
返回資訊列表
无码动漫av| 大香蕉网 久久| 97丁香婷婷| 女人露出p毛视频www网站| 拍色综合| 五月婷婷啪啪| 亚洲第一视频 久久| 精品日本视频444| 国内一级精品| 中文av网| 激情骚五月| 91婷婷搞| 99热6这里之有精品| 午夜激情四射影院| 97人人干。| 天天爽,夜夜爽| 久久99热这里只有精品| 日韩av干| 狠狠色大香蕉| 久久婷婷亚洲| 丁香五月综合在线视频| 久久免费丁香| 996热| 熟女色专区| 在线sebiav精品视频| 中文字幕av久久爽一区| http://www.sd-xiangsu.com/| 91vip在线观看| 日日夜夜狠狠| 干婷婷五月天| 99亚洲天堂| 日日夜夜爽| 97干在线观看视频| 在线观看玖玖资源免费观看| 天天爽天天摸| 六月丁香五月天| 亚洲天天| 91碰在线| 1999天天操夜夜操| www.五月婷婷.com| 九月丁香婷婷综合激情| 九九热99re8热免费观看| 丁香五月婷婷啪啪| www91色网站| 中文人妻AV久久人妻18| 色婷婷五月天亚洲| 人人摸人人干| 欧美日韩精品人妻狠狠躁免费视频| 另类视在线| 中文资源在线a | 婷婷激情五月天激情| 久久伊人大香蕉| 婷婷日本在线| 五月天堂六月丁香亚州中文字幕久久| 丁香五月亚洲综合| 性爱综合网| 五月婷婷天堂| 人妻久久久久久久| 三十熟女| 91刘玥视频在线观看| 亚洲久艹| 五月丁查人人| 99精品偷自拍| 99在线亚洲| 五月婷婷六月少妇激情| 色色五月天婷婷丁香| 日本欧美成人片AAAA| 99这里只有精品| 色亭亭影园| 一起草AV入口| 狠狠人妻色综合| 99热只有这里有精品| 91精品无码| 婷婷久久精品| xxxx五月天色色| 色婷婷久久综合久色| 99热官网| 久婷五月| 96精品成人无码A片观看金桔| 五月天色婷婷基地| 九九色综合视频| 久9免费视频| 国产av天堂| 夜色.cnm| 婷五月丁香| 第五色婷婷| 国产精品涩涩涩视频网站| 天天操天天国产三级片处女学生妹| 亚洲AVDVD| 呦呦视频无码播放| 激情综合色图| AA丁香综合激情| 狠狠狠狠狠狠色| 久久五月六月| 婷婷五月婷婷| 婷婷六月丁香激情| 婷婷成人丁香色情基地30 | 综合激情五月丁香| 天天色视频| 99热这是里只有精品| 激情六月婷婷| 激情深爱五月天| 天天做天天爱| 蜜臀99久久精品久久久久| 9久热在线视频精品| 六月激情久久婷婷| 99re这里只有精品在线观看| 91精品婷婷国产综合久久| 久久激情网| 国外亚洲成AV人片在线观看| 日韩综合天堂| 婷婷丁香五月高清| 九九色逼| 天天操中文字幕| 天天狠狠婷婷在线| 色婷婷久久| 深爱激情丁香| 激情熟女网| 欧美激情丁香五月| 开心五月网| 婷婷金品综合视频| 绿色小导航AV| 久久婷婷五月天激情唯美| 五月丁香六月| 成人小说色图婷婷五月| 男人综合网| 亚洲无码色| 丁香六月婷| www.com.色色| wwccc久久久| 五月开心久久| 色吧99| 色色五月天激情| 好激情在线综合网| 操操啪| 97se视频在线| 国产无套精品一区二区| 91婷婷视频| 五月天婷婷综合久久| 99久久久| AV大片在线观看| 婷婷丁香五月天色播网站| 五月丁香亭亭| 色九九九九| 这里有精品99| 天天干天天拍| 国产韩日亚洲美州欧亚综合在线| 色综合久久44| 五月婷婷与六月丁香图片激情| 丁香综合伊人AV| 日韩人妻无码专区| 九九综合视频在线观看| 99激情视频| 丁乡久久| 久久久久激情| 超碰在线人人| 婷婷五月草| 婷婷五月天淫荡| 五月亚洲激情| 人妻无码视频网| 婷婷六月丁香综合| 裸睡玩奶头(高H)| 国产激情久久| 色婷婷综合网站| 丁香五月激情啪啪| 九九热精品| 久青操| 七七色色综合| 亚洲五月婷婷| 婷婷精品| 天天干天天叉| 五月天丁香综合在线| 嫩草AV久久伊人妇女超级A| 午夜成人综合| 五月天婷婷视频| 久操婷婷| 第九色区av天堂| 五月色婷婷综合色| 天天日色情| 天天成人综合视频| 欧美123区免| 丁香六月久| 毛片蕉地一二| 九月婷婷| 色婷婷五月天天天干天天操天天爽 | 大香蕉婷婷丁香视频在线| 综合色影院| 欧美色97| 五月丁香婷婷AV天堂| 少妇被下春药玩弄A片| 婷婷五月 丁香六月| 99热这里有精品| 九九精品这里只有| 亚洲 欧洲 国产 伦综合| 激情五月,色播五月| 内射干少妇亚洲69XXX| 激情五月婷婷综合视频| 伊人五月婷| 婷婷五月天精品| 9精品国产在热久久| 五月天另类视频| 第四色网婷婷| 日本人妻伦在线中文字幕| 五月天激情中文字幕| 教师性爱毛片| 激情五月天综合| 天天爽,天天操。| 九九爱激情| 五月丁香婷婷欧美| 色五月婷婷大| 97色色色| 婷婷五月丁香激情图片| 天天操天天插| 色色五月丁香婷婷| 99热99热99热99热| 狠狠色丁香久久久婷| 夫妇交换刺激做爰| 婷婷色导航| 欧美五月丁香在线观看| 久久女人天堂| 五月婷视频在线观看| 99热6这里只有精品6| 色色色色色色色色网站| 久久五月丁香激情综合| 色狠狠综合| 刘玥精品一区| 久久思思热| 久久超视频| 综合激情五月丁香9999久久精| 丁香五月亚洲综合| 成人综合网站| 99精品免费| 亚洲va在线∨a天堂va欧美va| 丁香六月激| 亚洲成人精品三区| 熟妇人妻中文字幕无码老熟妇| 亚洲五月天另类小说图片| 玖玖伦理电影| 五月婷在线| 中文字幕精品推荐免费在线观| 激情五月婷婷五月| 色丁香久久| 五月激情婷婷女| 婷婷第六色| 91日综合欧美| 影音先锋AV资源男人站| 五月日韩中文字幕| 思思9久久| 亚洲激情在线| 五月丁香啪啪综合| 中文字幕激情综合| 播五月丁香三月婷婷| 色色色欧美色色| 六月婷婷天天操夜夜爽视频| 天天做天天爱天天综合| 婷婷日日天天| 日本女va| 婷婷激情伍月网| 婷婷五月天中文字幕| 色色五月婷婷网| 思思热在线| 激情四射网| 五月激情六月综合| www.激情五月| 丁香婷五月天开心六月| 日亚二欧美| 激情www| 99日这里只有精品| 啪到高潮激情丁香五月| 五月天天爱| 欧美大肥婆大肥BBBBB| 激情六月天| www.天天日| 婷婷五月天色| 色五月 五月婷婷| 天堂草在线看www| 久久久久婷婷五月热综合| ww久久| 久久久99婷婷久久久久久| 热久久这里只有三级视频| 热99视频精品在线| 五月婷婷先锋| 美女激情综合| 九九热只有这里精品| 另类亚洲2| 色婷亚洲| 五月桃花网综合| 麻豆AV一区二区三区| 日本色婷婷| 亚洲亚洲人成综合网络| 九九精品9| 偷拍五月丁香| 玖玖婷婷五月天毛片| 五月丁香婷婷钟和色图| 婷婷激情五月天小说| www91在线| 大香蕉婷婷久久| 操人91| 色婷婷视频| 99色在线观看| 五月婷婷色影院| 79精品视频在线观看,| 开心五月天激情网| 亚洲国产黄色电影| 99热网站| 性色视频| www.久热| 天天操天天国产三级片处女学生妹| www.一起草av| 五月天婷婷一起草| 欧美丁香婷婷五月| 99亚洲精品| 久久久天堂国产精品女人| 婷婷丁香六月天激情四射网| 亚洲综合1024| 色综合综合色| 99热九九热| 欧美婷婷色| 99手机在线精品视频| 五月婷婷干干干| 97色伦另类图片小说视频 | 天天射影院| 久久久久久久久久久久久久久久一道本| 丁香综合久久| 成人欧美一区二区三区在线观看| 婷婷五月丁香综合激情| 人妻熟人中文字幕一区二区| 久久久久久久久久91| 91一起操| 97碰人人操| 国产偷人爽久久久久久老妇APP| 婷婷丁香黄色| 黄涩毛片| 五月天激情小说| 97se在线视频| 伊人久久婷婷五月天激情四射| 99网| 91丨九色丨国产| 亚洲免费电影2| 亚洲中文无码成人| 久久色天堂| 五月日韩中文字幕| 婷婷丁香五月激情综合站_久久五月丁香激情综合_开心五月综合激情综合五月_婷 | 五月婷婷综合在线| 日本精品在线噜噜噜| 六月丁香激情婷婷| 色视频色综合91| 久久综合中文| 97碰| 久久性刺激| 日韩99色99| 欧美交换配乱吟粗大25P| 色色色色区| www,婷婷五月天,com| 另类视频五月天| 色天堂97| 色色色五月天激情资源| 激情五月综合六月丁香婷婷狠狠干| 国产婷婷综合| 91|九色|动漫| 九九综合色综合| 无码人妻少妇色欲AV一区二区| 97性视频| 色丁香五月| 午夜福利8055| 丁香五月偷拍| BT综合在线视频观看| 99r这里| 综合色图婷婷| 激情五月天色婷婷| 亚洲AV成人精品网站在线播放| 天天插天天很| 日韩限制级大尺度黑料泄密大尺度视频一区二区在线观看 | 色婷网| 色五月婷婷自拍| 99视频精品在线| 久草网大香视频| 激情婷婷五月天日本系列| 噼里啪啦完整版中文在线观看 | 久久这里只有精品8| www.91操| 99操逼| 婷婷六月丁香在线| 色吧综合网| 超碰操网| 无码免费人妻A片AAA毛片西瓜| 色五月婷婷五月丁香五月激情五月视频| 婷婷五月天综合网| 久久久免费精彩视频| 色综合久久综合中文综合网| 色色五月丁香| 亚洲一色色色色色色色色| 狠狠操狠狠操AV| 色色网站在线免费观看视频| 天天干天天av天天射| 天天肏天天爽夜夜爽| 久久丁香综合精品综合| 激情五月天综合网| 五月天大香蕉| 久操欧美在线观看97| 激情都市五月天| 97超级碰碰碰久久久| 久久这里只有精品热在99| 九月大香蕉| 五月综合色| 超碰人人99| 亚洲九九夜夜| 丁香涩涩五月天| 丁香97综合| 久久人人九九| 激情小说五月天中文字幕| 电影91久久久| 成人一级片| 成人色站,在线视频,看片-SS1AV| 爱狠射| 免费黄色视频网址| 国内精品玖玖| 亚洲永久四色| 国产AV网页| 熟女网站久久| 就爱射中文字幕资源网| 色三级色三级| 国模九区| 色情综合| 久久久久亚洲AV无码网影音先锋| 日操五月婷| 五月天堂色色| 被强行糟蹋的女人A片| 日本色婷婷| 天天噜天天爱| 91九色国产在线| 国产AV一区二区三区日韩| 五月天综合视频| 色婷婷AV在线| 日本97在线看片| 日韩啊啊啊| 九九色综合九九色| 欧洲亚洲欧洲99久久| 激情久久网 | 色5月婷婷| 五月天伊人久久久久| 日本激情ⅩXX免费视频| 色色色综合色| 九九精品婷| 久久怡红院| 婷婷五月情| 五月五丁香婷婷| 五月综合激情| 超碰国产AV| 色哟呦av| 国产毛片精品一区二区色欲黄A片| 思思久久99热只有频精品66| 亚州激情在线视频| 欧美婷婷六月丁香综合色| 久久九九亚洲| 日韩久久成人| 欧美激情五月天| 色伊人婷婷| 99热免| 亚洲AAA| 人人操99| 色999;丁香五月| 五月婷久久草| 无码髙清| 夜夜大香蕉婷婷丁香| 成人龟情网丁香五月| 9久热| 丝袜激情网| 五月天最新网| 人。妻久久| 国产片色| 丁香六月亚洲综合| 丁香久久久| 涩涩五| 小视频aaa久久久| 午夜丁香丁香婷婷| 色五月丁香总合网| 亚洲亚洲激情| 高清无码中文字幕aVDV| 亭亭社区五月天| 五月天婷婷乱| 涩涩涩.com| 丁香五月激情婷婷激情| 亚洲精品99| 色婷婷激情| 深爱综合网| 婷婷五月丁香五月| 色狠狠色噜噜AV天堂五区| 天天日夜夜| 亚洲精品白浆高清久久久久久| 五月丁香网站| 色吧网91| 神马欧美精| 一起草av在线观看| 婷婷五月天久久久| caobi四区| 这里只有免费的精品| 五月天夜夜爱夜夜操| 五月天婷婷永久免费视频| 丁香婷婷五月六月天| 婷婷五月天伦理| 天天干天天操天天上| 亚洲精品久久久久久久久久吃药| 激情综合色婷婷啪啪六月天| 日日操日日撸| 伊人五月综合网| 精品无码久久久久久久久| 日逼影音先锋男人AV资源站| 婷婷五月丁香色播| 欧美婷婷六月丁香综合色| 免费的日逼视频| caopeng97日韩| 日夜夜久久| 天天干夜晚夜操| 久久五月丁香| 拍色综合| 九九99在线观看视频| 啪啪婷婷五月天激情| 日本性激情色播| www.婷婷| 少妇2做爰HD韩国电影| 丁香五月欧美婷婷| 久久99久久99精品免观看粉| 婷婷五月天777| 2025年最新亚洲在线欧美| 综合性爱网| 国产露脸150部国语对白| 好色婷婷| 色六月视频| 国产无套精品一区二区| 91岛国片| 丁香六月毛片| 九九色综合| 操逼电影免费看| WWW久久久| 五月婷在线色视频| 婷婷综合色色| 99热99在线精品| 伊人五月天在线| 免看黄大片AA | 五月天久久综合| 高清无码中文字幕aVDV| 超碰大香蕉网| 激情五月婷婷啪啪| 深爱激情六月| 中文字幕无码AV| 久久九九怡红院| 性色天| 婷婷99视频全集高清| 97五月天婷婷午夜| 天天艹夜夜艹| 色综合com| 综合XX网| 大香av| 丁香婷婷五月| 五月天婷婷Av| 成人电影丁香六月天| 国产毛片精品一区二区色欲黄A片| 五月婷啪| 九九色热| 大香蕉综合网| 久草热在线视频| 九色视频91| 五月停停999| 色五月婷婷激情基地| 亚城区在线| 99网址在线观看| 东北婷婷五月天| 少妇人妻人伦A片| 五月丁香爱婷婷深深| 久久久久久久久久8888| 日韩成人电影AV| 伦乱天堂| 五月婷五月婷伊人伊人五月婷| 丁香五月香蕉| 人妻22p| 中文字幕永久在线| 伍月婷丁香婷| 成人片黄网站色大片免费毛片| 婷婷六月婷婷| 九色综合网| 性生活久久人妻| 亚洲国产黄色电影| 国产这里只有精品| 色综合久久99色| 无码任你操| 91久久久久久久| 五月天丁香六月综合| 色婷视频| 丁香美女主播视频在线观看 | 久久狠色噜噜狠狠狠狠97| 中文字幕日产A片在线看| 婷婷丁香五月激情图片| 成人在线免费网址| 五月 成人 婷婷| 91chinese在线| 日本人妻A片成人免费看片| 天天舔天天摸天天射| 亚洲六月综合激情久久下卡| 九九在线精点品| 99久在线视频| 亚洲精品无人区| 久久久久丁香婷婷五月天| 伊人色综合影院视频| 激情婷婷五月| 日韩精品无码99| 性热视频99精品| 婷婷激情社区| 操操操AV| 开心六月婷| 色色色色色色综合网| 丁香五月婷婷五月天| a在线观看| 一二线视频 另类| 能看的av网站| 天天插天天插天天日| 九七色色六月丁香| 久久久久9| 五月丁香人人婷婷在线观看| 狠狠爱婷婷丁香| 婷婷六月天| 五月天久久网站| 大香蕉伊人久久| 国产在线网址1| 五月色色色| 99色一| 亚洲av成人在线| 桃色成人网| 日本婷久久| 99热这里只有精品50| 超碰93在线观看| 中文字幕中文有码在线| 丁香婷婷久久 | 香蕉AV777XXX色综合一区| 这里有精品| 色青青视频| 91成人品| 婷婷五月天久久久| 久久色五月天激情小说| 亚洲va成人va成人va在线观看| 久久网思思| 丁香九月激情| 超碰国产AV| 激情内射人妻1区2区3区| 熟女网站久久| 欧美综合激情丁香五月六月婷| 亚洲五月婷婷在线| www.激情五月天。com| www.爱婷婷.com| 91久久精品国产91性色TV| 91操人视频| 99网| 久久婷婷综合五月趴| 日本婷婷丁香五月| 激情五月婷婷综合| 成人av在线网站| 日韩美女在线视频19| www.丁香六月婷婷久久天堂影院.con| 亚洲精品成人| 五月丁香六月在线| 99色综合网| 激情综合网激情五月天| 996日日爱| 大香蕉院线| 5月丁香综合网| 五月天婷五月天综合网小说首页-五月天激激婷婷大综合,婷婷亚洲综合五月天小说 | 99九九综合久久九九| 亚洲成人无码片| 丝袜熟女一区二区三区| 六月婷婷色色网| 五月网站| 久久九九激情五月天| 亚洲亚洲人成综合网络| 色五月激情五月| 欧美成人AAA片一区国产精品| 青青青在线视频国产| 91一起操| 亚洲激情综合| 91av传媒高清在线视频网| 色伦专区97中文字幕| 精品一二三区久久AAA片| 啪啪啪大香蕉| 亚洲视频在线观看| 2018夜夜草| 丁香五月狠狠在线观看| jiujiuxiangjiaowang| 夜色.cnm| 亚洲成人网站在线播放| 人人操97| 97亚洲色 torrent magnet| www.xtbsty.cn.com蜜乳AV| 韩日在线熟女| 九九AV| 欧美顶级少妇做爰HD| 国产又色又爽又黄又免费| 538在线精品| av网址在线播放| 久久激情四射| 九九干视频| 久热大香蕉| 九九热10| 成人五月天视频播放| 俺去也五月天| 色吧五月婷婷| co超碰在线观看| 天综合日日夜综合7799| 五月婷婷啪啪啪| 丁香五月综合久久八| 99热精品9| 天天日 天天草| 99热在这里只有精品| 国产精品久久久久久亚洲毛片| 婷婷操无码| 久久99网址| 99热在线免费观看精品| 婷婷五月综合亚洲| 色偷偷五月天| 黄色短视频在线观看| 影音先锋四区| 亚洲女婷婷五月基地综合久久久| 99热这里只有精品55| 91欧美| 亚洲精品第一国产综合亚AV | 亚洲AV影片在线观看| 婷婷五月丁香色播| Av性爱网| 亚洲激情另类| 欧美性猛交XXXX乱大交极品| 午夜婷婷久久 | 婷婷97碰碰| 99热在线中文字幕| 这里只有精品视频在线| 久热久| 夜夜爱影院| 就爱啪啪婷婷| 久久久999精品| 97啪在线观看视频| 色五月激情视频在线综合| 美英法精品无码免费视频| 97资源碰碰| 五月丁香婷久久| 天天做天天爱天天日| www.99热这里精品| 亚洲成人无码专区| 亚洲永久四色| 色五月婷婷网| 中文字幕不卡网站| 99色色网站| 亚洲不卡欧洲| 人妻中文av| renrencaoav| 色综合色综合网| 婷婷丁香综合| 2025年最新亚洲在线欧美| 综合五月婷婷| 色色婷| 色婷婷亚洲婷婷| 9色婷婷| 色七色九九| 色九区| 九九青草热| 婷婷五月综合视频免费播放| 字幕网AV中文字幕| 婷婷激情五月天7| 五月婷网| 婷婷中文无码| 天天肏天天肏天天肏| 激情五月天婷婷丁香| 精品少妇蜜臀91| 日日影院 | 婷婷五月丁香五月丁香| 欧美色色色色色| 欧美色色色色色| 欧美色九| av在线观看网站| 色五月欧美| 久久综合播放| 99激情| www.9797国产| 久久久27操| 97人妻碰碰碰久久| 婷婷丁香18| www.综合久久| 少妇人妻人伦A片| 殴美日韩成人| 中文字幕AV网址| 日韩 中文 欧美| 色色色国产| 五月婷婷六月天| 九色视频91| 五月丁香美女视频| 亚洲va成人va成人va在线观看| 色色日韩网| 91一起操| 天天干天天 亚洲| 97久久综合网| 26uuu欧美日本| 超碰色女| 99热久| wwxx日本| 91九色PORNY肉丝在线| 激情婷婷色五月| 九月色婷婷综合| 亚洲av成人在线| 色情性爱视频网址| 久久AV无码精品人妻系列试探| 五月丁香婷婷婷婷综合网| 婷婷综合九色伊人| 日本婷色| 99婷婷| 国产又色又爽又黄又免费| 激情五月天色婷婷综合| 麻豆精品| 泰州成人视频| 99精品在线下载| 热996精品在线观看| av在线观看网站| xxxx五月| www,99色| 欧美这里只有精品| 五月丁香亚洲综合网| 日韩无码91| 思思热99热| 久热只有这里精品| 日本久久人人| 99ER热精品视频| 婷婷五月天激情电影小说| 人人操AV| 激情综合色五月丁香| 婷婷丁香五月婷婷| WWW.激情| 美女91一起草| 天天插天天射| 51国精产品自偷自偷综合| 丁香婷婷激情综合五月激情 | 色激情五月天| 天堂草在线观看| 色噜噜狠狠色综合日日| 久久九九激情五月天 | 亚洲色涩视频| 伊人五月天久久| 久热久| 日本狠狠色| 十月丁香婷婷| 五月天丁香综合久久国产| 香蕉久久国产AV一区二区| 婷婷五月天视频| 婷婷丁香五月天色区| 婷婷九月亚洲| 六月婷欧美丁香综合| 色色色色色色色色综合网| 婷婷五月天六点丁香五月| 丁香六月婷婷综合激情欧美| 五月天大香蕉AV| 农村熟妇高潮精品A片| 操逼视频一区| 综合伊人狠狠| 激情六月五月婷婷综合网| 色综合久久中文| 久草x色在线观看99| 丁香桃色网| 少妇人妻人伦A片| www.sezonghe| 26uuu淫色| 欧美色色色色色| 碰碰碰91| ai97re99一本| 欧美性猛交99久久久久99按摩| 97人人操人人爽| 66成人网| 婷婷五月天精品| 天天做综合网色综合| 久久视屏这里只有久久| 9久热在线视频精品| 色婷综合| 亚洲天堂婷婷| 精品国产一区二区三区四区阿崩 | 婷婷五月花| 天天天干夜夜夜操| 五月丁香婷婷色色| 日韩精品一区二区亚洲AV观看 | 亚洲AV成人在线| 91色吧网| 久久九九@| 99热精品在线在线| 丁香五月激情在线| 五月成人天| 99免费视频网| 久久人妻视步| 色综合色综合网| 激情五月丁香六月婷婷| 亚洲Av入口| 五月丁香六月婷婷中合网| 密黄站| 无码一区二区三区四区五区| 偷偷狠狠久久婷婷五月天| 粉嫩av懂色av蜜臀av熟妇| 一起草无码| 99热亚洲精品| 7超碰自拍| 色久九| 婷婷综合在线| 26uuu国自产精品| 日本综合久| 99热 在线观看| 婷婷五月天,影院| 99热这里只有精品13| WWW、日本色丁香、co m| 狠狠肏综合网| 五月天婷婷永久免费视频| www,五月天激情| 天天撸天天干天天插| 狠狠狠狠狠狠狠狠| 婷婷婷婷婷婷婷婷| 久久玖玖综合| 操操碰| 五月天伊人| 色婷插| 九九超日本| 青青草色在线视频观看| 婷婷香蕉| 情欲综合网| 操逼棍操逼| 五月婷婷啪啪| 一级性爱视频| 色婷婷基地 | 中文字幕综合色| 五月丁香网站| 天天 青草 制服丝袜 在线| 婷婷色香六月综合激情| 日日夜夜狠狠干| 激情九九六月激情免费视频| 色播五月| ztEJj| 99干日本| 六月丁香婷婷色综合| 色色网五月激情| 日本精品99网站| 五月天开心网| 一本九九色| 激情四射五月天| 小视频一区| 狠狠干在线| 狠狠干婷婷| 色玖玖网| 91超级碰| 狠色色狠网| 丁香五月性| 国产性爱大片久久| 国产成人网站在线观看| 丁香五月婷婷亚洲另类| 婷婷月综合| 青柠影视免费高清电视剧| 色香久久| 亚洲瑟瑟精品在线| 欧美色色色色色色色色色色| 国产精品久久久久久久久久| 亚洲色婷婷五月天| 狠狠色官网| 久久丝袜婷婷| 久久久婷婷五月天| 色五月天丁香| 国产裸舞表演WWWW| 五月色综合| 中文字幕丰满人妻无码专区| 天天拍久久| 色婷婷网| 九伊人网| 色情久久久| 99热这里只有精品55| 美女伊人久久| 色婷婷五月天激情综合| 午夜免费试看| 色婷婷影音| 丁香五月激情月| 人妻AV在线| 中日韩狠狠色| 五月婷婷六月基地| 91操碰| 国产ava| 国产五月天欧美色| 亚洲乱码日产精品BD| 丁香色五月 97干| 五月综合婷婷五月| 五月婷婷 激情按摩| 婷婷五月在线综合| 六月激情婷婷| 色欧美影院| 综合激情站| 丁香五月婷婷社区| 婷婷丁香五| 色99在线视频| 激情综合网址| 成人免费在线电影| 久久婷婷五月综合一| 亚洲99在线| 国产成人精品123区免费视频| 色色色激情| 色区久久| 欧美综合五月丁香六月婷| 婷婷丁香十月| 综合激情九月婷婷,激情综合婷婷中文字| 欧美激情xxxXX| 久久久久婷婷| 色综天天综合| 狠狠干无码| 色综合久久天天综合网| 久久九九@| 99人人操人人操人人精| 激情都市丁香婷婷| 97成人超碰免| 色欲一二三| 欧美三级视频下载| 天天爽天天摸天天爱| 国产欧美大香蕉一区| 国产资源91在线| 五月丁香狠狠爱婷婷综合| 综合色综合| 97超级啪啪在线观看| 五月情涩综合婷婷| 久久五月热| 99视频在线精品免费观看2| 99re视频在线精品| 精品色| 久操干| 91啪啪视频| 操操操操操操婷婷五月天| 高清无码入口| 色噜噜狠狠色综合日日免费| 久久人妻熟女一区二区| 色色日韩无码| 婷婷狠狠香蕉综合| 六月份天丁香婷婷| 玖玖五月| 伊人在线视频| 中文不卡一二三区| 中文字幕在线视频播放| 五月激情六月综合| 久久综合无| 亚洲激情综合| 亚洲尤物在线| 久久一伦| 日本啪啪天堂| 99久久国产成人精品| 人妻第九页| 五月婷婷官网色| 97色婷婷成人综合在线观看| 97人人射| 热99AV网站| 婷婷五月综合社区| h在线看免费版在线看| 激情综合网,婷婷五月天| 色色97丁香婷婷五月天| 五月丁香久人妻中文| 激情综合五月激情XXXX| 天天干在线播放| 亚洲无码99| 99久久99视频只有精品| 日韩国产在线精品| 99色色网| peg 2区三区四区的| 丁香av网| 婷婷基地爱| 激情五月婷婷综合网| 海外网站专业操老外| 99精品热| va婷婷| 色情婷婷| 5Www色5夜| 日韩操人| 婷婷色5月激情网| 久久九色| 久久日婷婷| 日韩无码色色| 丁香五月激情在线| 只有精品在线观看| 91久久国产综合久久| 精品久久久人妻| 99热这里只有精品官网| 日本啪啪天堂| www.婷婷久久五月天| 九九久久综合| 五月激情六月宗合| 久婷婷五月丁香在线观看| 亚洲无码免费看| 天天做天天爱天天爽| 五月丁香啪啪| www.婷婷五月天啪啪| 丁香婷婷色九月| 婷五月天影院| 五月天丁香| 五月天成人在线播放| 丁香五月亚洲综合| www.天天色综合| 91疯狂操操操操| 97成人操| 天天干一干| 久草大| 九九婷婷五月天| 天天操天天操| 久久精彩免费视频精彩免费视频| WWW.五月com| 无码AV免费精品一区二区三区 | 久久久网站| 91AV婷婷| 婷婷网五月天| 这里只精品热在线18| 丁香五月婷婷姐| 91丨九色丨国产打屁股| 成人丁香五月婷| 五月天婷婷爱丁香中文字幕| 色婷婷五月天成人网| 天啪天啪天啪天啪| 桃色成人网| 五月婷婷综合网| 亚洲操b| 九九热中文| 九九热这里| 久久丁香久久| 国产高清精品色| 另类图片 五月激情| 丁香丁香激情网| AV中文在线| 亚洲天堂爱爱| xxxx久| 九月婷婷激情| 九九亚洲无码| 激情婷婷丁香色五月| 国产原创视频91九色| 综合色在线| www.色五月| 极品人妻VIDEOSSS人妻| 国产暴力强伦轩1区二区小说| 99视频在线精品| 亚洲男女激情| 中文av网| 久久精彩免费视频| 99热这里只有精品8| www.色婷婷.com| 大香蕉啪啪网| 人妻丰满精品一区二区A片| 九九伊人网| 人妻体体内射精一区二区| 噼里啪啦在线观看免费完整版视频 | 丁香五月婷婷AV| 五月天婷婷av| 激情六月综合| 99久久婷婷| 伊人综合网站| 五月天色色无码| 99色在线观看视频者| AV中文字幕夜夜操b天天摸bb| 99caobi| 99ri精品在线| 色欲av伊人久久大香线蕉影院| 97在线精品视频| www.激情五月天。com| 99热国产这里只有| 六月婷婷影院| 九九这里都是精品| 干一干xxxx| 久久九九网| 亚洲激情综合| 亚洲一区二区色图-亚洲精品国产精品乱码-成人AV | 六月色国内综合| 九九视频在线观看| 99精品热视频| 五月丁香久久| 婷婷五月天免费99| 97人妻碰碰碰碰碰久久久久久| AV动漫不卡无码免费| 婷婷放心五日爱| 三级成人网站| se99视频| 国产精品成人在线| 亚洲色图五月丁香| 五月激情天天干| 99热e| 久久精彩免费视频| 五月桃花网综合| 亚洲无aV在线中文字幕| 婷婷五月色播网| 五月激情基地| 成人丁香色| 国产精品涩涩涩视频网站| 日本nghangse中文字幕| 欧美在线97| 久久只这里有精品| 五月婷在线| 久久97| 免费AV在线网址| 激情桃色网| 久久精品视频在这里有| 香蕉综合网| 五月天婷婷网站| 色5月婷婷| 丁香五月天天高清在线| 亚洲愉拍99热成人精品| 欧美A级成人婬片免费看理论| 激情图片久久| 狠狠色官网| 99这里只有精品| 国产精产国品一二三在观看| 久热亚洲| 五月丁香 啪啪啪| 婷婷视频网| 91碰免费视频| 人妻性爱av网站| 裸体美女丁香五月天。| 久久婷婷五月综合| 天天操五月天| 九九亚洲小视频| 一起草AV| 人人综合91网| 亚洲XX日本| 91丨九色丨大屁股| 超级碰 久久9| 99区视频| 成人AV在线网站| 婷婷94s| 婷婷五月天777| 亚洲婷婷91丁香| 色婷婷五月综合色婷婷| 亚洲精品操一操、噜一噜、摸一摸、爽 | 狠狠爱五月婷婷| 欧美婷婷色五月| 被强行糟蹋的女人A片| 亚洲av成人在线| www夜夜操| 玖玖婷婷色欲| 激情五月婷婷欧美极品| 亚洲综合婷婷| 久操97| 热99在线| 丁香亚洲婷婷五月| 67194成I人在线观看线路1| 欧美日韩一区二区三区四区| 婷婷在线操| 狠狠色综合网| 操B五月天| 999久久久国产精品| 色99xx| 五月婷婷综合潮喷| 成人做爰A片免费看视频| 天天射影| 婷婷五月天色| 人人摸人人操人人爽| 丁香五月天婷婷激情| 韩日AV片| 999影院成人在线影院| 国产在线aaa片一区二区99| 开心五月天私房婷婷| 六月丁香激情网| 超碰激情网| 色色色色色色色色五月先| 9久国产| 99re在线视频| 婷婷爱爱蜜臀天天操| 99爱视频| 偷偷操99| 99在线免费视频| 秋霞日本免费毛片A片| 97色色综合| 久热中文字幕| 超碰av在线| 精品婷婷五|