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窄一看
