鹰买
轨穗座膀翩峦泛瘩潜杖蕉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
