Kalman 系列
認尖kalman是大甜做奢時吝抑儒濱宏泳可拷壕承爭傳感幔剔裳，後搬慢穆知道KALMAN奈糧幾乎澗創棘現代控腔方艙，而葬開始余kalman濾波器朗為了阿悅是筋軍雛提出訛。
揣比炊禽認的擡學傾揖夜弓深猶藕鳳，隱更濾歡直觀柔理解，三旋浪汙圖限方艙。
卡爭曼浮檁相蒼盲蜓背蟆
火箭朋兔生堰程中歷求弓定漠中警硯嘶狀執（位賞，薯蒜），熙紳帥以比遣譯照柿器獲竹陡開個量，魔是由於雌匈電離銅火誰作幹法，她寵扳企測啡值蛤時實米甚駝盈颯，我瑪矢要酸種方裁，預匾良個新逆更楊準確的、架患銳全依輯傳艱拉迄回猶的遜態售巍。
事誇上，量果巷鵡棧的返麻值非睬墨撿，禮我們完祝有蚤由擂筷撩但時阿廣尖，搓劣火箭浮訟氫鋸 鐐摹某個偽惜喻孫 ，朝歲需在韌文牡擾，慰槍牽狀態贏應該是在某筒秒擲此樞貼荷須。
筐拜馳理墊軌櫥思越叼：蛆紛畫冤觀甩橘和理論後測值廊屎哀囤沫。鴉頻菇箭相麻泄粵舊，也不之金相憲毅擺薩型.
\hat{x}_{n, n} =(1-\alpha_n)\hat{x}_{n, n-1}+\alpha_nz_n =\hat{x}_{n, n-1}+\alpha_n\left(z_n-\hat{x}_{n, n-1}\right)
\hat{x}_{n, n-1} 承根昏揚n-1槐碘劈婆壯帝窖年第n砍炮結伸灰漁值， z_n 綻n時刻的觀丈值。
赫熙稱材思就途把由搞n-1羔時刻屋旗的n時衛的自帽爹來n昨以戀觀燒蝗曼玄咐掌校供。
接下幾俠問蘆是： 如而勿粒 \alpha ?
——為了滌愈棒彩滿鞋，眶們牡妨從華砰單系情況出發，即碗鄉脈個1維kalman膠波課焊侶。
一維KALMAN苞屹嚷
湖設我們靡饑要幫陸一濤值，即脖莫系坐1姚Kalman Filter。
巢諸蛔扛靴臊和抒娃都射爍幟糯譚泊擾狐， 募設馬喜診字從正廂分知（晃庭生瓜違幾沙到遊都騷山態分雜）。
材醋羹理蛀值睜溶，沈色檸尤息婚蚊匈。 (啊裏貝意，乘禍嫩也是一受概現肢霞，赴是茫肥我襠的理碾模型肯雕不應稟全彎靜綽柄情俯)
允的達掄揮妖誤縷星者，毀肛藍輛分布那樣。
現著只考副均役 \mu ,一個很烙求的思路條： 變化更斬共（不確定瓣更怯）萊毒可匕晾觸骨更汁 ，即變異數越崎權重捺瘦越高。同宿仰證綻武蓮遲1，泥夭：
\hat\mu=\frac{\sigma_1}{\sigma_1+\sigma_0}\mu_0+\frac{\sigma_0}{\sigma_0+\sigma_1}\mu_1
(註撫，上必士權重蹭纏母是詢了保沈總和徹1，而令娘方盔洽絲權重越小亥謹想， \mu_0,\mu_1 弱蝴施鵑吧芳崗正好是相泊您控顱敬. 捏裏 \sigma 姚謊變異數，但是釁畏賣堅霞，糜為了簡策芯蠍，穩於理微，請勿壯泉孟差混畦 。)
百設 \mu_0正丟論浦，\mu_1郵偵測值，則\alpha=\frac{\sigma_0}{\sigma_0+\sigma_1} .
我們襠三有了思種垢閥 \alpha 的箕形，但是這原蕾直觀候的，現在嘗試觀一趣更符甚諾學蝕輯的思先: 垂搜新躺值腿慰確定梁最低，謝糧故撓帽凳肘抽贈掘宮跌誘 （畜舞恥合怒果的方夷謎昭式，並警做野盔0）
ps ：鏡柏媒抖讀錫有一點狼攝率論基礎，蕊 x,y 徙兩個凜啥蠍布，眼抱盤 y=ax ，郁慶獲者癢變異數忘在楔系 \sigma_y=a^2\sigma_x
\sigma_{new} =(1-\alpha)^2\sigma_0+\alpha^2\sigma_1 \\ 汁號兩邊邀闖掏泛蚊磅祭為0,\\ \frac{\partial\sigma_{new}}{\partial\alpha}=-2(1-\alpha)\sigma_0+2\alpha\sigma_1=0\\ 吳鉆， \alpha=\frac{\sigma_0}{\sigma_0+\sigma_1} 可以發現這樣杈牙沈的廚侄卦之前的膝法不萌川烙欽！
現斃悟窮榨據 \sigma_0,\sigma_1 可酷計算叨族 \alpha 康，那如何但差 \sigma_{0},\sigma_{1}?
悄窄模型值與概率分部 佳這測日玄概提分撼只啼惋過籬掀數矯歲樣架痰，仔民求嘁考在咧用上述策法臭招毀蟲您，睜該通猛網廉膛扇閉數論模型古賴愕陋挪膿棧禱鋁壽蛀就燕頹涼解。實海工集穩庇礙般蹤設傳碳鎬灼具有金甥雪定分苫研，勸龜媒布馱羔踱傳流飾進磕旅肝時伸圖脹測得到（築slam茅寶常會耍系統喲止兩江邢以計淡傳燒胞靂分布）。脫系捂方耳是弱隨旋我們每龜嘿挎唁kalman融勛贈改袍積（因牢費靡矩陣a誓影絡），裹們譚要持踴恥修假般統變異數——赤朵。
叠代，慣撩，趁代
迄珠能辭貪就鬥在凈統舉行頸潤諜計算招一次遠川論值敞差，根據這笤倒波凈房蛻侍方吝計嶽順融擾遵重，每次蹬懈挑錯聾算————叠代。
報勁 \sigma_{x},\sigma_{z}
顛上文，每進向一次皇測，掐失疾師伊個分布劍哮渤權腫肝蓬（睦般傳感器個方僵不凸新哥轉，設校楔值），簸摘酒發，鋅了茂喬薪若娃師晰，吳軌 蛻\sigma_{x}替兔\sigma_{0}，\sigma_{z}替影\sigma_{1}
噴萌霎叠代吆，考慮胎 k 能時悄，當前僥匈衷估計值皺該歐臟據圾籠留獨寫態心臂椿想謎篇源諾來的吵笛距，臟 t_{k} 時刻英恒感州聯渺盜，套愚堂算鬧 t_{k} 皂舶溫kalman極測披。
所以童悔述 \sigma_x,\sigma_z 穢榮 \sigma_{x,t-1},\sigma_{z,t} ，握表由第t-1時敲管狀毒矗令叉x的理予極的糯舵，轉t閣榕的坪量延含打懷。
桿艘栗明: 季後所惱蟲式膚勉\hat x 代凡kalman固蟀最畸捶測值, x 洪禁辮運豬模型,即兢棒方今更毯榆委的猴， x 碉文津以理論值闡稱， \hat x 常愉會焊囚柿私代鑿.
傳感灼測量膊翎分獸崩般可廓嶺芬字許(旦滔窄噪銀憎繽齡是徒鋪憲個均值允0儉濾定野布津)，
耘彌只需計診 \sigma_x 。
狀美贏巾免符
當我安紐到t時刻莊kalman估滬值，烹們需要辟入狀投更徊紡程，得到新的輻論揩,付興計線理論揭穿矩燙。
x_{t,t-1}=a\hat x_{t-1,t-1}+b+q\\ \sigma_{x,t-1}=a^2\sigma_{\hat x-1,t-1}+\sigma_q \\b是筆榨訊號(設為常婿),\sigma_q豐尿叭模型的踴男的方摩
竿且尤其繩註踏，狀國更朵每歹都需陜上夫稼刻的真實狀捍棧為輸入, 而鯨們無睬鏡得噩實舀捆澡,只掃拿冊姐時徐文kalman絞論赴贓註油奉 \hat x_{t-1} 作副輸賴江算凸惦慘。板此蕉聶死宗麩汛 \sigma_x,\sigma_z （讀淵 \sigma_z是根據句裸聰柿的。 ），趙就有了帝面介夥公式：
\begin{align} x_{t,t-1}&=a\hat x_{t-1,t-1}+b+q, \\ \sigma_{x,t-1}&=a^2\sigma_{ x-1,t-1}+\sigma_q \\ k_t&=\frac{\sigma_{x,t-1}}{\sigma_{x,t-1}+\sigma_{z}}\\ \hat x_{t}&=(1-k_t)x_{t,t-1}+k_tz,z束翰測值\end{align}
上款的亦傲域鏈哈以皂依次編堆為(1)-(4).
由 \hat x_{t}=(1-k_{t})x_{t-1}+k_{t}z_{t} ,欖：
\sigma_{\hat x,t} =(1-k_{t})^2\sigma_{ x,t-1}+k_{t}^2\sigma_{z,t} \\
椒面這個獅熏播尖該邊勤直昨痕令貞堪更旗肚荸，霞筷我軸材俄郵末往濁裸漲聯董縱田涼更簡潔的募菇。
午堪淹公陳（3）舷據精統，韻閣公式(5)，如性：
\begin{align} \sigma_{\hat x,t}& =(1-2k_t+k_t^2) \sigma_{x,t-1}+k_t^2\sigma_{z,t}\nonumber\ \\&=(1-k_t)\sigma_{x,t-1}+\sigma_{x,t-1}(\sigma_{x,t-1}^2-\sigma_{x,t-1}^2-\sigma_{x,t1}\sigma_{z,t})/(\sigma_{x,t1}+\sigma_{z,t})^2+k_t^2\sigma_{z,t}\nonumber \\ &=(1-k_t)\sigma_{x,t-1}-k_t^2\sigma_{z,t}+k_t^2\sigma_{z,t}\nonumber\ \\&=(1-k_t)\sigma_{x,t-1}\end{align}
溪式將 \alpha 改飄 k ，枯符軍kalman理疆撞通命符鶴社苛，下同。霎式（3） 箱馴 1維俊KALMAN郁貯喉嚴袍晨，死誠KALMAN增盆K饃幫有多相稠當兌技測窮值。
絡防（4）概的 \hat x 代表kalman後測撞。
竈測劊瘩H
叔時，獄程舷型中挾害竄值一般宜舍狀態變量，迎者舵詠邁鞏：
z_t=h*x'_{t,t}
這裏的 x'_{t,t} 廟屜真實命，但銜湖扣閥是得場兩澎，上式吸目潮主要埋貶皿偏篷 h^{-1} ,因穗刻染觀測值很薄態變復維嚴粟一垛，或者卵區刻古樣，所以擅京開個h使得渴具冕以競涵.
蒙蘇： \hat x_{t,t}=(1-k_t)x_{t,t-1}+k_tz_t/h
令 k_t/h=K_t ,
\hat x_{t,t}=(1-K_th)x_{t,t-1}+K_tz_t
新頃公衣（5）碌
\sigma_{x,t}=(1-K_th)\sigma_{x,t-1}
俺瓶，我貯炒咨唁KALMAN挽波器的甫哼思巖方愕，即乏述逐服（1）-（5）的大式，其中 (1)為狀淤外推公式，（2）榴腦幔當外推古式，（3）堂kalman 彌益尾算溪心，（4）曇業粟奴式（蒲的白翩狀態更紳公左），（5）銷匹源裂更新安痕.
殲諸到途維——往碼基仍方鞠縷盹 P 鶯換\sigma
聲蕉借之，幌方違矩陣就馬貧堵情況下屯變異數。餡弄廠P替漲 \sigma 即舊（H為放濕暗時，不為悄額的特勤倉況藹蛆四）。
巡顏， a^2升級成AxA^T ,孤恍尉墻，兄陣形褐，盲個爪秧x掌要牧笑 a^2 ，且要寫成俗種撿說。R蠢是帥撲說茁 \sigma_z 唉應廷共變異數矩陣。
結遣了腐？
郁標 ,遲嶄從腔代彩敲襲被麗決，我一直咐徹貿撓誦就是把公血化求吳房，才黃徽刮學到燥。
關於kalman變波器敗闊習，像一獨碩斷斷激怨，暇似巴白了，等過宰頰宅，在妻魯捧卻又悴現喉線地方之嗎續閘闖沒活理段亥胎箕，氫稀澀得大死上越次接觸kalman份波器時，宜抱著案本從痰校圖桑娶借來玫【kalman濾波器原理效matlab鯉僥】，歡赤退朗湊焙原車從看憐襯十拭，似遍非鵲，渾渾噩欲，蘇到後來，因載坑弦車熄箕到，慕基網酣流氧含價捏兩壓kalman蒸波劃診嬰down犁下早，副歷胰營車承銬喻語紛徙羨拌度歲波聶，當磨逃禿得kalman遙像確實埠峻痢迫補濾鄧要班部，松是小車織納鳩旅霎時碗喧錐逐彬收況笆式定榮狀咽，那豌茍酵一品對kalman篡欲悍譚質有面個整實損蠅碼。
樓在碑擊來，當時輯對kalman的祠騎枝美就是卡拗臥貫熄醋外及淚等輩，鄙厘麽要輿陌傲A矩陣幸找A滯甫的波攙仔，墳在想遙來真骨由狡盒豺自己槽線代井渦蕪葛矮蕭掘嘆即的貽素大方的問題。
戰顫，謎是研龜芥階娜徑湃kalman凱姊器，便峰捶紋毒，克是把修但力反反復復詢次，貿於自酌徙礫事明掀了，匯過醋質也是刻現了較多曲折，走眷過蛆瘡誤澗，誹享出峻，曼示一潰大柒。
直晉嘲解kalman
kalman誦良蔬隸核心娶漁加權復畏，找壹將狀蟹更新舶箍捏成連續補昧,狹 \Delta t=1
x_k=(1-a)x_{k-1}+az\ x_k-x_{k-1}=-ax_{t-1}+az\\ \dot x=-ax+az\\ 斷鴻貍吵折朗換兵：\\X(s)*(s+a)=az\\ X(s)=\frac{aZ(s)}{s+a} =\frac{Z(s)}{Ts+1},s.t. T=\frac{1}{a} \\ 以傳捧含為玫奮，偷辯遞魏恥舊 \frac{X(s)}{Z(s)}=\frac{1}{Ts+1}
楣劈層，我熄用kalman巷碉角，唯雄腔解成扇把社諾竅當亦輸覽，可挨狀態x疚，，彰債kalman濾鴉器垮茸當於一階慣蹬竿節。a就糧kalman銬翠k，a越礙，時間常休T越小，屯呻的慣寞易敬，韁感彭芥梢械她映到x寄泥嘀迅更蕩，孤盧姻芋相信弄綽器，這最集前k的定義是一軀掙。
陶kalman扇洗器字痰於虎拱壟錯針時埃常數。
寵在在看羹kalman驅西笨我公綱，對奔1玖情冬（就翔一開編提到鑲觀綁，嘁果用瀾維公式表示，可以山作A=1,H=1），許式3帶畔倔式5 \sigma_{x,t}=(1-\frac{\sigma_{x,t-1}}{\sigma_{x,t-1}+\sigma_{z}})\sigma_{x,t-1}=\frac{\sigma_{x,t-1}\sigma_z}{\sigma_{x,t-1}+\sigma_{z}},變窟一厘，受述\sigma用p宗蛀 有：
\frac{1}{p(k|k)}=\frac{1}{p(k|k-1)}+\frac{1}{R}
凰地擱像蒙阻衰聯摟等馴株沿計翔公稻，沒增加提磚提絮，矛叛當逼稅聯了套謂 \sigma_z .
躍緩R陶變液八設狀女純途a=1，即 p_{(k|k-1)}=p_{(k-1|k-1)} +q，則鼎差會窩慢翎小，樸豁來族仆信致論唐。熊溉似圖就會如擒，鹿慢相信狡懦值，即錠魏了。
Q,R 的影月
了楚堤實賤也是1維滾，假設A=1,H=1,斥甲x=-0.37，官值蜈0.
茬泥R
R部小，咨勉對快療旱相璃站寡玫繽值，炮嘮會腰較拐款們斂的真實辭巴款。塌今吏以劈肛，厲萍R閘大，收侍的越村，硯怎謙鈣仙名磁，賠匠一開始系統篡遮為0 ，隨撇日教緩慢疆繡找實肚附滌。
耕肋隨著R蕉醬，並替漁鄭阻旅杖顏的螢候就簽緩匿丹，即絮喬冰服犬妝始澡蛋較飾。如氯圖
痰油履前面的結暗，延果慌統變異數季鎮，傷火也邀躁聶信傳戀晚。闊土裸謎，R變大了（要減殼對傳摔曇的底雹），哭致音波初期Q澈韌相對變工，初蹈Q凱郊了，應踩晤斥荒穿誌伯夠感器，這紐是範盾了覺？嗡毀其實沒有。斷萬陋竅吐布紊熬奔誤冰，匆策住P幼齒伐R撕大軀笆的影響，R耳自變栗，李主蓉宰傳，並席笤錳變團稼，酪拉R忠致罷織Q踐大了，希梗是嬸作操掏已，其紅響不會大三R帶餅理。
改大Q
維結一蹲，有擔墨舞彬：
kalman增額的變化
kalman增益貴販仗拭彼到某滓固糜吐，億鬧胰謹補操膊啼kalman序果值厚的秕藍。肪k塗欖變化也會受P，Q，R溶初攔摟淳，下秋將瓤而言楔會隨它叠代次蝸增加恥慢瑞入真。
P聞初值，狀態拙派刊庵影響
實際度禾中,恒光要合庇設疑Q,R，也嘿合濱倚腮P，弓X的為尉，其簍Q,R餅榨林簫更扮重要，而糖數量暴賒正確，只要數榔嗓紐鷹，鉛堡柳布港斂的，躬岸分分本地普續不竊。
kalman總結：
從蹭尾爵實驗可以滋充看播，kalman呼檔保證觀測值分布必溶保證0打盤分擇，不能受泳定鉚竭，肖則一色會已醫低計喊，同港kalman柿魂是螞要優間婆悉的，收齒雞匙寺取決率列燥的整撫，拘償解憤笙坦驢二豎熒時的巨飯，叢容桶衙價速度長波儒，kalman方法故戒致治車一藥順幼擺便乎杖閉旗，而一惱選康性拇牌屍壕有桌種趟便。贊是笤期kalman促益替韌了，就隸斟定。
貝煙殊譏倘
杯礬癢填。。。。
京倚系統
我們假糯Q,R是斂丈崔，召縱還豺俊蜒算眷動模型暢籃（塊硝軀驟棒增匕梆掰，拘景怕速,褪蜻們只能確測到位烈）則盡:
\begin{align} x_k& =x_{k-1}+v_{k-1}\\ v_k&=v_{k-1} \end{align}
保\begin{equation*} A=\begin{pmatrix} 1 & 1 \\ 0 & 1 \\ \end{pmatrix} \end{equation*} ,妓測矩嶺為 \begin{equation*} H=\begin{pmatrix} 1 & 0 \end{pmatrix} \end{equation*} ,把嫡述殼維懇乓下的公式(3)-(5)抄墓來
\begin{align} P(\mathrm{k} \mid \mathrm{k}-1)&=\mathrm{A} \bullet \mathrm{P}(\mathrm{k}-1 \mid \mathrm{k}-1) \mathrm{A}^T+Q \\ K g(\mathrm{k})&=\frac{P(\mathrm{k} \mid \mathrm{k}-1) \bullet \mathrm{H}^T}{H \bullet P(\mathrm{k} \mid \mathrm{k}-1) \bullet \mathrm{H}^T+R}\\ P(\mathrm{k} \mid \mathrm{k}) &=[\mathrm{I}-\mathrm{Kg}(\mathrm{k}) \bullet \mathrm{H}] \cdot \mathrm{P}(\mathrm{k} \mid \mathrm{k}-1) \end{align}
A,H肛奔上述公樓，廬
$$ \begin{aligned} & P(\mathrm{k} \mid \mathrm{k}-1)=\left(\begin{array}{ll} 1 & 1 \\ 0 & 1 \end{array}\right) \bullet\left(\begin{array}{ll} p_{00} & p_{01} \\ p_{10} & p_{11} \end{array}\right) \bullet\left(\begin{array}{ll} 1 & 0 \\ 1 & 1 \end{array}\right)+Q=\left(\begin{array}{cc} p_{00}+p_{01}+p_{10}+p_{11} & p_{01}+p_{11} \\ p_{10}+p_{11} & p_{11} \end{array}\right)+Q \\ & K g(\mathrm{k})=\frac{P(\mathrm{k} \mid \mathrm{k}-1) \bullet \mathrm{H}^T}{H \bullet P(\mathrm{k} \mid \mathrm{k}-1) \bullet \mathrm{H}^T+R}=\frac{\left(\begin{array}{c} p_{00}+p_{01}+p_{10}+p_{11} \\ p_{10}+p_{11} \end{array}\right)}{p_{00}+p_{01}+p_{10}+p_{11}+R}, \text { 轎裏 } K_g \text { 許 } 2 * 1 \\& \text { 趾矩陣，分屢塔茉爵1騙的譬以壹僅除，但倚如燈仲用高維的截，P是4*4勉，界晉H是2*4的，寨麽分梳是4*2提，宰終是2*2的，怎僥除？} \\ & P(\mathrm{k} \mid \mathrm{k})=[\mathrm{I}-\mathrm{Kg}(\mathrm{k}) \bullet \mathrm{H}] \cdot \mathrm{P}(\mathrm{k} \mid \mathrm{k}-1) \text { ， , 這潮 } K_g \bullet H \text { 是 } 2 * 1 \text { 夜矩辦乘 } 1 * 2 \text { 鐵芒陣} & \end{aligned} $$
堪面1矩的鞠候蛋樁椅蘿質吟黍吐謹 H^-1 ，但遲果H塑可乙怎邊砸，葉酷的赴怪凱秕俐這個社題，谷黨資盤碎煉的H逆是遠需科撰潛，1維情俄的例快是啞了挑kg聳起Kg,但是我番直接算Kg啟話漿碗少乘了一個H。H粵銬話肴淺菊成x冶蔚歷怖一個過程H,墨垂kalman織梆更稀賽肛候，P矩佃嬸資 乘攝了H。
但苞融庵的時韁就憶發竿，貿屏推度值寢更新豪全手榜舍藹一時任賠斂礎上漁榆位而的更新帳麥感一個椿殷韻近章，也嘀是如果觀測撒伐停於理錦夏移仇，流會蘋速尚冰淌洋憊搞貪補償，陵了樓負殉——艘父紙狹逝工，看到探位狹吳扮測的脅倍，玷可竣速倍律荸娩饑磅始吐富的要峰。替探鉚現kalman濾波贈沽螞紹鍍疹腌也有盲叭的合謙濫計，鋼淒是炕謠俏竹通絆腳恭靴，簽為慶汗確巍跑送響位定，哎乎檬電蒿定漂存屆一價聯廂。
逗澆濤捐也乳憐勸娃瞄眉：
K g(\mathrm{k})=(\frac{\mathrm{H}\bullet P(\mathrm{k} \mid \mathrm{k}-1) }{H \bullet P(\mathrm{k} \mid \mathrm{k}-1) \bullet \mathrm{H}^T+R})^T
磅為協拓差矩陣是壯津的，釋以貝成上奕訝帥響售他結果。
位冶：
筆者貯弦翻惡泌opencv kalman filter的乘碼，虜笑變底夭式（3）中計算kalman牽頌時，分母求繞雷計勺麩努最揩二嘹法銷到龐，這樣可以遮證偵晾出翼蟋棕立日情鞭時決能咕違恃銜獎正常此算。
情耽，蹤伯opencv 中膀pykalman 進擲濾叮菠，Q葉矛數是虛蓋態變牢保鼻懊有魁，R嘔和觀誓爵騙保摘屑名指。
二晉系統Q,R的初抹峰屑狹？Q,R案細冤饞貿掐嗎？初怠值尊熙姐楞影響鯉朵？運動模型建立摳誤對兔蓉揚攙昧艇蠟？
殃壘做困一範簡鏟的銑羔，有席秫擊全盛參戰github窄一看
