一看就懂的卡尔曼滤波五个公式

任何物体的运动都有一个运动方程：
xk^=Axk−1+u+w,w∈(0,Q)(1)\hat{x_k} =Ax_{k-1}+u+w ,w\in (0,Q) \tag{1} xk^=Axk−1+u+w,w∈(0,Q)(1)式中，k为某一个时刻，u为输入量，x为k-1时刻系统真实状态,w为方差Q的高斯误差，xk^\hat{x_k}xk^为k时刻估计状态。
至此，根据这个系统状态我们有一个测量方程：
zk^=Hxk^(2)\hat{z_k} =H\hat{x_k}\tag{2} zk^=Hxk^(2)式（2）被人称为先验。
这两个方程由于实际上很多情况达不到标准导致了会有误差，然后方程变为：
zk=Hxk^+v,v∈(0,R)(3)\\z_k=H\hat{x_k}+v,v\in (0,R)\tag{3} zk=Hxk^+v,v∈(0,R)(3)式中，v为方差R的高斯误差。
一般来说，式（3）中的zkz_kzk是通过传感器测量出来的（测量值），式（2）中的zk^\hat{z_k}zk^是我们通过运动方程转化过来的（理论值）。针对这两个值我们到底应该相信谁呢？当然是谁的表现好谁的权重大，从而可以建立一个方程：
HXnew=(1−K)Hxk^+Kzk=Hxk^+K(zk−Hxk^)(5)HX_{new}=(1-K)H\hat{x_k}+Kz_k=H\hat{x_k}+K(z_k-H\hat{x_k})\tag{5} HXnew=(1−K)Hxk^+Kzk=Hxk^+K(zk−Hxk^)(5)式中，HXnewHX_{new}HXnew为后验，K为权重。然后会发现K如何取值，前面说了谁的表现好就取谁的多一点，而衡量一个好学生表现好的标准是他成绩好然后保持稳定，成绩好就是卡尔曼算法的初值需要好，稳定就是协方差要小，说白了就是需要保持在90分以上而不是一会儿70一会儿100的。所以我们需要对协方差建模，首先是xk^\hat{x_k}xk^的协方差pk‾\overline{p_k}pk通过（1）式可得：
pk‾=Aek−1ek−1′A′+Q=Apk−1A′+Q(6)\overline{p_k}=Ae_{k-1}e_{k-1}'A'+Q=Ap_{k-1}A'+Q\tag{6} pk=Aek−1ek−1′A′+Q=Apk−1A′+Q(6)然后zk的协方差pk~\widetilde{p_k}pk也可以根据（3）求出：
pk~=Hek^ek^′H′+R=Hpk‾H′+R(7)\widetilde{p_k}=H\hat{e_k}\hat{e_k}'H'+R=H\overline{p_k}H'+R\tag{7} pk=Hek^ek^′H′+R=HpkH′+R(7)而先验协方差pk^\hat{p_k}pk^很容易从式（2）得出：
pk^=Hek^ek^′H′=Hpk‾H′\hat{p_k}=H\hat{e_k}\hat{e_k}'H'=H\overline{p_k}H' pk^=Hek^ek^′H′=HpkH′从而我们可以建立一个权重表达式：
K=pk^/pk~==Hpk‾H′/(Hpk‾H′+R)(8)K= \hat{p_k}/ \widetilde{p_k}==H\overline{p_k}H'/(H\overline{p_k}H'+R)\tag{8} K=pk^/pk==HpkH′/(HpkH′+R)(8)从该市可看出理论值的协方差pk^\hat{p_k}pk^越大，K越大，式（5）中理论值的比重越小，间接说明了式（5）的正确性，把K值代入式（5）我们可以得到：
HXnew=Hxk^+Hpk‾H′/(Hpk‾H′+R)∗(zk−Hxk^)(9)HX_{new}=H\hat{x_k}+H\overline{p_k}H'/(H\overline{p_k}H'+R)*(z_k-H\hat{x_k})\tag{9} HXnew=Hxk^+HpkH′/(HpkH′+R)∗(zk−Hxk^)(9)但是我们想要估计的是状态X_{new}，设K′=pk‾H′/(Hpk‾H′+R)K'=\overline{p_k}H'/(H\overline{p_k}H'+R)K′=pkH′/(HpkH′+R)，从而式（5）可以两边除以H可得：
Xnew=xk^+K′(zk−Hxk^)(10)X_{new}=\hat{x_k}+K'(z_k-H\hat{x_k}) \tag{10} Xnew=xk^+K′(zk−Hxk^)(10)式中。XnewX_{new}Xnew估计值与真实值X_t的误差为：
xt−Xnew=xt−xk^−K′(Hxt−Hxk^+v)=ek−K′Hek+K′v(11)x_t-X_{new}=x_t-\hat{x_k}-K'(Hx_t-H\hat{x_k}+v)=e_k-K'He_k+K'v \tag{11} xt−Xnew=xt−xk^−K′(Hxt−Hxk^+v)=ek−K′Hek+K′v(11)从而更新协方差方程：
Pk=（I−K′H）pk‾(12)Pk=（I-K'H）\overline{p_k} \tag{12} Pk=（I−K′H）pk(12)
随便弄个跟踪图：

一看就懂的卡尔曼滤波五个公式相关推荐

卡尔曼滤波五个公式_基于ROS的卡尔曼滤波姿态解算
前段时间由于项目关系需要实现基于卡尔曼滤波的姿态解算,也就是说融合加速度计.陀螺仪及磁罗盘进行AHRS姿态解算,得到机器人的姿态角. 本文的学习需要有一定的卡尔曼滤波器基础,可以参考白巧克力亦唯心的卡 ...
一看就懂！卡尔曼滤波通俗解释
假设你有两个传感器,测的是同一个信号.可是它们每次的读数都不太一样,怎么办? 取平均. 再假设你知道其中贵的那个传感器应该准一些,便宜的那个应该差一些.那有比取平均更好的办法吗? 加权平均. 怎么加权 ...
卡尔曼滤波五个公式各个参数的意义
系统的状态方程为: 这个状态方程是根据上一时刻的状态和控制变量来推测此刻的状态,wk-1是服从高斯分布的噪声,是预测过程的噪声,它对应了 xk 中每个分量的噪声,是期望为 0,协方差为 Q 的高斯白噪 ...
卡尔曼滤波的五大公式及python代码示例
文章目录一.系统的状态方程二.观测方程三.五大公式四.Python代码一.系统的状态方程状态方程是根据上一时刻的状态和控制变量来推测此刻的状态 xkx_{k}xk 是状态分量的 nnn维 ...
《假如编程是魔法之零基础看得懂的Python入门教程》——（五）我的魔法竟然有了一丝逻辑
学习目标了解魔法世界中的结构表现--缩进了解魔法世界的逻辑判断--if 了解魔法世界的多次逻辑判断--ifelse嵌套了解魔法世界中的逻辑运算--且 and 与或 or 推荐 1.<备受好 ...
c++ 结构体赋值_《零基础看得懂的C语言入门教程》—（十二）结构体是这么回事
一.学习目标了解C语言的结构体的使用方法了解C语言结构体的结构的赋值了解多种C语言结构体变量的赋值方法和取值方法目录 <零基础看得懂的C语言入门教程>--(二)简单带你了解流程 & ...
《零基础看得懂的C++入门教程》——（7）小数组玩起来
一.学习目标了解数组的使用方法了解一维数组的使用方法了解一维数组与循环的使用方法目录预备第一篇,使用软件介绍在这一篇,C++与C使用的软件是一样的,查看这篇即可:<软件介绍> 想 ...
《假如编程是魔法之零基础看得懂的Python入门教程》——（七）我把魔法变成了积木
学习目标了解魔法积木的使用--自定义函数了解魔法积木的结果反馈--自定义函数返回值了解魔法积木的原料传递--自定义函数传参了解魔法积木的类型分类--类与对象推荐 1.<备受好评的看得懂 ...
《假如编程是魔法之零基础看得懂的Python入门教程》——（六）精简魔法更强大
学习目标了解对相似逻辑的简化编写--循环推荐 1.<备受好评的看得懂的C语言入门教程> 目录第一篇:<假如编程是魔法之零基础看得懂的Python入门教程 >--(一)既然 ...

一看就懂的卡尔曼滤波五个公式

一看就懂的卡尔曼滤波五个公式

一看就懂的卡尔曼滤波五个公式相关推荐

最新文章

热门文章