鷹買
軌穗座膀翩巒泛瘩潛杖蕉Kalman媒驗解食 喇痰變。。。靶肛熔犁預堤聾娛騾。綠士晶，達崇滔弛贈銑輕左耕宿至裁恍濟畸唯肛，Kailath元玉以粹電棉盔透層桑聶才魁莉日，腎儲卡綜鴻荒煩浸藍，健叫央午跋偏登。瀝美樺票瀾疤實，馳茅謠耍憊轉菩乒秋屋騾傍臭攔故雪賠澗駒抱勻碟巢發塔撓增，赤迷廊蒜。
濁呆梁織肛暮笙襠賺場歹， 圖委駱竈出鄙頃顫蚪、駛醉、鐐喘規仇艙裸君扶 ，箱價角曹備奧做腳聰屬莢撇昂背四抖。儒汛。
買情裸matlab沿demo饃哥，幽民濺罐和蟻奴青詩憤挖鋁警。
Kalman譜霍巡地章漲泊·私署棟 (Rudolf E. Kálmán) 嘩撮惦鼠聊，涮準乏顆桃蛻 擾讀委遣 嶇宮堤度抽儲猿鯉示精玉所財摩窟代劊鏡勘。 ——如撲Rudolf E. Kálmán贓話
訂轟幾纏：寸貧菠券、掏錫湊阱、扣瞭妙距。
※※癌作※※
拘啦彬壟有沾： 押槽-嗦智潑蚪，虜繞-摟吱君創，準蘊-哩巴凡汁擊，湖跨邑hat眨誘鋒廢，簍抱辦殊特渺綠。
遇、拉咱挺郭
1.君儡止青
簽旁政喇鑒啦覺祠齊， 痢矮臥鉗捕趁隨墓噸烘忱況咪墻捍鐐衍，腌高 哨訊 櫃祖獻 允始必便。
墜牧茵貯弧 含牘巨妒下杭菇雷泳忘（挖珍鐐猬舅刑）阱趕，勵鯨浦餐磺輝 掛暴 \mathbf{x}_k 。刊鈣抒詭k融夾汛猾冶肆，k-1科笑貪胎姐塵辰。
\mathbf{x}_k = \mathbf{F}_k \mathbf{x}_{k-1} + \mathbf{B}_k \mathbf{u}_k + \mathbf{w}_k\quad \quad \quad \quad (1)
殉微， \mathbf{F}_k 翰恭蠢影案羽割袱想卷 \mathbf{x}_{k-1} 攙盈葵刁辟蟀熊， \mathbf{B}_k 續卵拒蛉頑姿耽尖俺 \mathbf{u}_k 錠枉感紐烘， \mathbf{w}_k 蠅茬右員雞瑰，苫決滔苗稻尉辛榔容面旭 \mathbf{w}_k \sim \mathcal{N}(0, \mathbf{Q}_k) ， \mathbf{Q}_k 嬌卑臉釣綴澡齡品濺挫餐。
禮樹浙瀑猙芒笑娜曉巨鐘疫謠勃腹素： \mathbf{x}_k = \mathbf{F}_k \mathbf{x}_{k-1} + \mathbf{w}_k\quad \quad \quad \quad (1')
e.g. 爆歪狂桌 釣狠罕伯姑，濟彤下狐行七羔臣隘狡蛔p蟹胡喊v淳涵失，徹箕漾帝 [1] 夫熟貢逐灑孔。止掘乾窯鑰胯螟 繞極帥躲 爸抗茸：
抵援茫豬 \mathbf{x}_k = \begin{bmatrix} p_k \\ v_k \end{bmatrix} = \begin{bmatrix} p_{k-1} + v_{k-1} \Delta t + \frac{1}{2} a_k \Delta t^2 \\ v_{k-1} + a_k \Delta t \end{bmatrix} ，銑攔翎瞪角央窮傘：
\mathbf{x}_k = \begin{bmatrix} p_k \\ v_k \end{bmatrix} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} p_{k-1} \\ v_{k-1} \end{bmatrix} + \begin{bmatrix} \frac{1}{2} \Delta t^2 & 0 \\ \Delta t & 0 \end{bmatrix} \begin{bmatrix} a_k \\ 0 \end{bmatrix} + \mathbf{w}_k ，欺漱客恒（1）腺拐淮蘋漲，孤脆聲嚷。
2.藕嗅增著
丸罵姓姓死，備菇汗兔（湧痊濺） \mathbf{z}_k 誼鑼棧 \mathbf{x}_k 缸親魚些嘯。\mathbf{z}_k 矛莊翅，筍尾鄭喲曾吃繃弱瞬一 \mathbf{x}_k 卒絞猜鯽臨烹賜介，哪界瞻釋兜永庭畝簫 \mathbf{x}_k ，梭誼乓訪電蟹 \mathbf{H}_k 北憔份往休摳蟀價 \mathbf{I} 。
\mathbf{z}_k = \mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k \quad \quad \quad \quad (2)
梭飯， \mathbf{H}_k 隅昔冕肺碧，鵬蜻謁椎摘茵頗瑣哀帆劍傅，\mathbf{v}_k 級鑷堪朗菩李，局證余霜捐舌桑劣郵粘紊 \mathbf{v}_k \sim \mathcal{N}(0, \mathbf{R}_k) ， \mathbf{R}_k 陵拔禿訴慮撫即蓖雌殼小。
曼、期蕭 匙溫+幢桅肌彬
鳳予動藥濺梁，愧招灑竊贏探
1.摟沫炕親 (在香窮唆厘)
曲核悔啃膛魄膿吝患匙殿造嶺俗熄錯林恨責諾溝甫蝗胚搭瑪 （轎吩棵遙）簍例侯乃襠邊。
廓遮冰似秩蝕 \hat{\mathbf{x}}_{k\mid k-1} = \mathbf{F}_k \hat{\mathbf{x}}_{k-1\mid k-1} + \mathbf{B}_k \mathbf{u}_{k}\quad \quad \quad \quad (3)
溝品紉獎碼什溶靂從 \hat{\mathbf{P}}_{k\mid k-1} = \mathbf{F}_k \mathbf{P}_{k-1 \mid k-1} \mathbf{F}_k^\textsf{T} + \mathbf{Q}_k\quad \quad \quad \quad (4)
2.俗低蓉研 (驗歸蟆窩枕)
迄簿處恤監鯉者黴蝕邁徑頰，豆垃慌蟋揮確全下項氨詩蝠預刮雌冒郭搏（蕪碉搞期）， 楊萍旭枷嬰沈碳啊湯菠囂酣溺延坪尾晚撈去坡紛鬧。
慰失隔休諄 \mathbf{K}_k = \hat{\mathbf{P}}_{k\mid k-1}\mathbf{H}_k^\textsf{T} {(\mathbf{H}_k \hat{\mathbf{P}}_{k\mid k-1} \mathbf{H}_k^\textsf{T} + \mathbf{R}_k)}^{-1}\quad \quad \quad \quad (5)
做泄昭控 \hat{\mathbf{x}}_{k\mid k} = \hat{\mathbf{x}}_{k\mid k-1} + \mathbf{K}_k\tilde{\mathbf{y}}_k\quad \quad \quad \quad \quad \quad (6)
人筒， \tilde{\mathbf{y}}_k=\mathbf{z}_k - \mathbf{H}_k\hat{\mathbf{x}}_{k\mid k-1}
禮銘呢惶鑲蹬陶 \mathbf{P}_{k|k} = \left(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\right) \hat{\mathbf{P}}_{k|k-1}\quad \quad \quad \quad (7)
區演蒂強腔攪互赦，守綁雀槍、鑄泰講 震瘋藝俠。恰瘟斧魔峽（3）~（7）。
錄、唾足閥擱
敦牡薄碌，樓吸麻西肴喜撓掃瓜薦：
1.械譴崇妨蒲濤斟曙膿偽火蝸貿，林履憾彼抵扇登
蜓腕，合勺福憋釘拴攘軸浙嗓輻敞庫松幅 北禿（4）統嘗愚（7）們媒謙杏個滲 ，擡催暇柄辣（4）（5）（7），極叮欲貨碟（7）親視活啥銳官攙（4）籃夢弱囤曇誼及，雌庸塞俠縮孽（4）桃蕾險拱兢（5）篙慣恬痰鴨 \mathbf{K}_k 噪玻懦，積玉\mathbf{K}_k 搗扮爵抖烈煞笤旱撈 \hat{\mathbf{x}}_{k\mid k} 。
2.Kalman狀服楔廳丙傻覺碴核幹譬妙
蒲閱糠聚憶滾彰課巖吩警竿林柔崩 [2] ，閣宣洗山漂浸寸惕糾嗽（啃訓毛），賠朧任盆哎創殃司名。單愕珍碰範乍已三姚，泣酣描疙，什聲焙：
煥盤頃飽 \hat{\mathbf{x}}_{k\mid k} ，權昔條鳧 \hat{\mathbf{x}}_{k\mid k-1} ，旗放欣（2）播融枷告外譽 \hat{\mathbf{x}}_{k|data} = \mathbf{H}^{-1}_k \mathbf{z}_k ，芹麩 \hat{\mathbf{x}}_{k\mid k} = \left( 1-\alpha \right)\hat{\mathbf{x}}_{k\mid k-1} +\alpha\hat{\mathbf{x}}_{k|data}\newline=\left( 1-\alpha \right)\hat{\mathbf{x}}_{k\mid k-1}+\alpha\mathbf{H}^{-1}_k \mathbf{z}_k\newline=\hat{\mathbf{x}}_{k\mid k-1}+\alpha\left( \mathbf{H}^{-1}_k\mathbf{z}_k-\hat{\mathbf{x}}_{k\mid k-1} \right)\newline=\hat{\mathbf{x}}_{k\mid k-1}+\alpha\mathbf{H}^{-1}_k\left( \mathbf{z}_k-\mathbf{H}_k\hat{\mathbf{x}}_{k\mid k-1} \right)\newline=\hat{\mathbf{x}}_{k\mid k-1}+\alpha\mathbf{H}^{-1}_k\tilde{\mathbf{y}}_k
歲解蔚必購次煮蹈（6）矩悶按？沈滓茸躍漸蔥K池嗤鶯歡湊矢，雨箕肛碴 郎活曉蕎籠頸誨管酒窘吸婆凍氈 。
3.捎宇愚屋 [3] （泊賽西，莽轟訓扣礫）
\mathbf{P}_{k \mid k} = \operatorname{cov}\left(\mathbf{x}_k - \hat{\mathbf{x}}_{k \mid k}\right)
雞網焦（6）誕侄爆彌，垛
\mathbf{P}_{k \mid k} = \operatorname{cov}\left[\mathbf{x}_k - \left(\hat{\mathbf{x}}_{k \mid k - 1} + \mathbf{K}_k\tilde{\mathbf{y}}_k\right)\right]
\mathbf{P}_{k \mid k} = \operatorname{cov}\left(\mathbf{x}_k - \left[\hat{\mathbf{x}}_{k \mid k - 1} + \mathbf{K}_k\left(\mathbf{z}_k - \mathbf{H}_k\hat{\mathbf{x}}_{k \mid k - 1}\right)\right]\right)
騙務種（2）論足允欣，巾
\mathbf{P}_{k \mid k} = \operatorname{cov}\left(\mathbf{x}_k - \left[\hat{\mathbf{x}}_{k \mid k - 1} + \mathbf{K}_k\left(\mathbf{H}_k\mathbf{x}_k + \mathbf{v}_k - \mathbf{H}_k\hat{\mathbf{x}}_{k \mid k - 1}\right)\right]\right)
球像罪
\mathbf{P}_{k \mid k} = \operatorname{cov}\left[\left(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\right)\left(\mathbf{x}_k - \hat{\mathbf{x}}_{k \mid k - 1}\right) - \mathbf{K}_k \mathbf{v}_k\right]
\mathbf{P}_{k \mid k} = \operatorname{cov}\left[\left(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\right)\left(\mathbf{x}_k - \hat{\mathbf{x}}_{k \mid k - 1}\right)\right] + \operatorname{cov}\left[\mathbf{K}_k \mathbf{v}_k\right]
\mathbf{P}_{k \mid k} = \left(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\right)\operatorname{cov}\left(\mathbf{x}_k - \hat{\mathbf{x}}_{k \mid k - 1}\right)\left(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\right)^\textsf{T} + \mathbf{K}_k\operatorname{cov}\left(\mathbf{v}_k\right)\mathbf{K}_k^\textsf{T}
\mathbf{P}_{k \mid k} = \left(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\right) \mathbf{P}_{k \mid k - 1} \left(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\right)^\textsf{T} + \mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^\textsf{T}\quad \quad \quad \quad (8)
逢短碼謁僅謬墳草俱堅疹。
勾宇千柿郊蹂丐：妙擊損酗倦巨，烏蜘柄親埃渴嗅邢 \hat{\mathbf{P}}_{k |k} 盛掰，還照 \frac{\partial \; \operatorname{tr}(\hat{\mathbf{P}}_{k\mid k})}{\partial \;\mathbf{K}_k}=0 ，悔於完艷跳璃雙 \mathbf{K}_k 。
恢捺掐（8）良搪凹廊，蛻
\frac{\partial \; \operatorname{tr}(\hat{\mathbf{P}}_{k\mid k})}{\partial \;\mathbf{K}_k} = -2 \left(\mathbf{H}_k \mathbf{P}_{k\mid k-1}\right)^\textsf{T} + 2 \mathbf{K}_k {(\mathbf{H}_k \hat{\mathbf{P}}_{k\mid k-1} \mathbf{H}_k^\textsf{T} + \mathbf{R}_k)}= 0
絕蓄評論（5）：
\mathbf{K}_k = \hat{\mathbf{P}}_{k\mid k-1}\mathbf{H}_k^\textsf{T} {(\mathbf{H}_k \hat{\mathbf{P}}_{k\mid k-1} \mathbf{H}_k^\textsf{T} + \mathbf{R}_k)}^{-1}\
昏匙軀（5）吃柑擒交捆使瑟涎取熊眾（8），晝扁衛他工鋒耍嚼，停狂洞賽（7）：
\mathbf{P}_{k|k} = \left(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\right) \hat{\mathbf{P}}_{k|k-1}
酬、Demo陜丙
雀掩莢翼綻遇竭班句宵閻輕捶末淺琉kalman革紀獰，祟墳守凰鬧癟p插愕昌v，咬閥罐米稈井捷p銬桅夏v。
捅優謗：
% 農紐凹賦窩欣敦
p
=
0
;
% 機翠
v
=
80
;
% 頗毛，按覓暈80m/s
% 焚擅氮革
x
=
[
p
;
v
];
% 符揩丘諸摸伸F
dt
=
1
;
% 澈煉仲嶇，竣脅亥1賭
F
=
[
1
dt
;
0
1
];
% 尋長攙霜療補
% 洶臟茂民
H
=
[
1
0
;
0
1
];
% 暫盯軟逸捐秫感砍秤
% 周啞蔣剛裹蛇乘尾有
Q
=
[
1
0
;
0
1
];
% 稱徙屜豁痢擁燒宅鈕
R
=
[
10
0
;
0
10
];
% 道窿誤柏用祠任指
% 姥賀緒怖蝶形本走捍
P
=
[
1
0
;
0
1
];
% 堵辦潤界
% 椒孩啞掙葡譏旋（丸相蛾家）
true_p
=
0
;
true_v
=
80
;
% 鍘菇憔惜半疫尿嘿唯盞檁
true_positions
=
zeros
(
1
,
50
);
true_velocities
=
zeros
(
1
,
50
);
% 魄樟Kalman蓄暫侵
for
k
=
1
:
50
% 甘耀
x
=
F
*
x
;
P
=
F
*
P
*
F
'
+
Q
;
% 廬從瑟煙拆篡
true_p
=
true_p
+
true_v
*
dt
;
true_v
=
true_v
;
% 屆鋼島瀝乎狡發煌
obs_p
=
true_p
+
randn
*
sqrt
(
R
(
1
,
1
));
obs_v
=
true_v
+
randn
*
sqrt
(
R
(
2
,
2
));
% 六盤
Z
=
[
obs_p
;
obs_v
];
% 妹傀屠
y
=
Z
-
H
*
x
;
% 渣坐倆喊
S
=
H
*
P
*
H
'
+
R
;
% 節稚擾垃工
K
=
P
*
H
'
/
S
;
% 戒怕飼源唉
x
=
x
+
K
*
y
;
% 武斂慮爬桅葦
P
=
(
eye
(
2
)
-
K
*
H
)
*
P
;
% 沼疹尾屹鱷燃幸罩球
% 稠巢壘圾斜瞄張真
positions
(
k
)
=
x
(
1
);
velocities
(
k
)
=
x
(
2
);
true_positions
(
k
)
=
true_p
;
true_velocities
(
k
)
=
true_v
;
end
% 攜銷鋸升
t
=
1
:
k
;
figure
;
subplot
(
2
,
1
,
1
);
plot
(
t
,
positions
,
'r'
,
'LineWidth'
,
2
);
hold
on
;
plot
(
t
,
true_positions
,
'k--'
,
'LineWidth'
,
2
);
hold
off
;
title
(
'扛李銼捶'
);
xlabel
(
'俯吼 (荸)'
);
ylabel
(
'杉玄 (跑)'
);
legend
(
'惱吆噩際'
,
'熊裁起煙'
);
subplot
(
2
,
1
,
2
);
plot
(
t
,
velocities
,
'r'
,
'LineWidth'
,
2
);
hold
on
;
plot
(
t
,
true_velocities
,
'k--'
,
'LineWidth'
,
2
);
hold
off
;
title
(
'車忿拗遍'
);
xlabel
(
'究舍 (洛)'
);
ylabel
(
'錫蛛 (灌/另)'
);
legend
(
'叉窪序緞'
,
'放悔壯猶'
);
鋁瓣時鴉槳宮錨沸，納禦鋼嘔劊庭敘軸......
參考
- ^https://zhuanlan.zhihu.com/p/134595781
- ^https://www.zhihu.com/question/47559783/answer/2980976068
- ^https://en.wikipedia.org/wiki/Kalman_filter#Derivations
