卡尔曼滤波原理(一)

卡尔曼滤波是一种递归线性最小变量无偏估计原理：

1>无偏估计期望值等于真值 E(g')=g 其中g'是g的无偏估计

2> 最小变量估计 D(g')=E((g'-g)^2) if D(g') <= D(g~) 任意的g~ 也就是方差任意小

协方差矩阵表示随机变量间的关联

Xk=φk-1Xk-1+uk-1+τk-1Wk-1 (1) Wk-1 和Vk 是过程噪声和观测噪声

Zk=HkXk+Vk

估计器：X'k|k=K'k*X'k-1|k-1+Kk*Zk+K''k*uk-1 (2) 计算 Kk' Kk Kk''的值使得X'k|k是Xk的最优线性估计

X''k|k=Xk-X'k|k 最优估计值与真值得偏差 (3)

联立(1) (2) (3)则有下面表达式：

X'k,Zk,X'k|k——> X''k|k=φk-1Xk-1+uk-1+τk-1Wk-1-X'k|k-K'k*X'k-1|k-1+Kk*Zk+K''k*uk-1

——> X''k|k=φk-1Xk-1+uk-1+τk-1Wk-1-X'k|k-K'k*X'k-1|k-1+Kk*(HkXk+Vk)+K''k*uk-1

——>X''k|k=φk-1Xk-1+uk-1+τk-1Wk-1-X'k|k-K'k*X'k-1|k-1+Kk*HkXk+KkVk+K''k*uk-1

——>X''k|k=φk-1Xk-1+uk-1+τk-1Wk-1-X'k|k-K'k*X'k-1|k-1+Kk*Hk(φk-1Xk-1+uk-1+τk-1Wk-1)+KkVk+K''k*uk-1

——>X''k|k=(φk-1-Kk*Hkφk-1-K'k)Xk-1+K'k(Xk-1-X'k-1|k-1)+(τk-1(1-Kk*Hk))Wk-1+(In-Kk*Hk-K''k)uk-1-KkVk

如果： E(X''k|k)=0 E(X''k-1|k-1)=0 E(Vk)=0 E(Wk-1)=0 然而： E(Xk-1)=0 E(uk-1)=0 不恒等于0

所以：令φk-1-Kk*Hkφk-1-K'k=0 In-Kk*Hk-K''k=0消去其中的两项，则有：

K'k=φk-1-Kk*Hkφk-1 (4)

K''k=In-Kk*Hk

将(4)代入到(2)中，则可得Xk的无偏最优估计值为：

X'k|k=X'k|k-1+Kk(Zk-Z'k|k-1) (5)

X'k|k-1=φk-1X'k-1|k-1+uk-1,Z'k|k-1=HkX'k|k-1

——>求解Kk的最小值，使得 X'k|k是Xk的最小变量估计，最优化问题对Kk求偏导可得：

Kk=Pk|k-1Hk(T)(HkPk|k-1Hk(T)+Rk)^-1 (6) 其中In为单位矩阵 Hk(T) 转置

Pk|k=(In-KkHk)Pk|k-1

以下是卡尔曼滤波法的更新方程：

1.状态估计

X'k|k-1=φk-1X'k-1|k-1+uk-1

2.协方差矩阵

Pk|k-1 =φk-1Pk|k-1φk-1(T)+τk-1Qk-1τk-1(T)

3.卡尔曼增益矩阵

Kk= Pk|k-1Hk(T)(HkPk|k-1Hk(T)+Rk)^-1

4. 状态估计方程更新

X'k|k=X'k|k-1+Kk(Zk-Z'k|k-1)

Z'k|k-1=HkX'k|k-1

5. 误差协方差更新

Pk|k=(In-KkHk)Pk|k-1

总结：卡尔曼估计来源于经验和测量

卡尔曼滤波原理(一)相关推荐

自动驾驶算法-滤波器系列（一）——详解卡尔曼滤波原理
详解卡尔曼滤波原理什么是卡尔曼滤波? 我们能用卡尔曼滤波做什么? 卡尔曼滤波是如何看到你的问题的使用矩阵来描述问题外部控制量外部干扰用测量量来修正估计值融合高斯分布将所有公式整合起来总 ...
ukf源程序 matlab,《卡尔曼滤波原理及应用-MATLAB仿真》程序-5.1UKF
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 无迹Kalman滤波在目标跟踪中的应用 % ...
卡尔曼滤波原理及实现
卡尔曼滤波原理及实现前一段时间,做项目研究了一下卡尔曼滤波,并且在项目当中实现了一个物体跟踪的功能,所以,借着新鲜劲儿,本次博客对卡尔曼滤波进行一次整理. 卡尔曼滤波是什么卡尔曼滤波能做什么卡尔 ...
卡尔曼滤波原理详解（一）
卡尔曼滤波原理详解(一) 前言数据融合的思想例子引入卡尔曼增益推导前言本文是对卡尔曼滤波学习的记录,主要参照了DR_CAN老师的视频进行学习.视频专栏链接:DR_CAN卡尔曼滤波视频专栏.虽 ...
卡尔曼滤波原理及c++实现
卡尔曼滤波原理及c++实现滤波原理卡尔曼滤波是一种最优估计算法. 用处:1)利用可测量值估算无法测量的量.2)对有测量噪声的物理量进行估计.常用于制导和导航.计算机视觉等. 状态估计器卡尔曼滤波 ...
卡尔曼滤波原理详解（二）
卡尔曼滤波原理详解(二) 前言卡尔曼增益推导总结前言本文是对卡尔曼滤波学习的记录,主要参照了DR_CAN老师的视频进行学习.视频专栏链接:DR_CAN卡尔曼滤波视频专栏.虽然网上有很多卡尔曼滤 ...
《卡尔曼滤波原理及应用-MATLAB仿真》程序-5.2UKF
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 功能说明: UKF在目标跟踪中的应用 % 参数 ...
《卡尔曼滤波原理及应用-MATLAB仿真》程序-5.3UKF
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 程序说明:对比UKF与EKF在非线性系统中应用 ...
《卡尔曼滤波原理及应用-MATLAB仿真》程序-2.2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 程序说明:练习使用plot函数,线型设置 % ...
卡尔曼滤波原理公式详细推导过程[包括引出]
卡尔曼滤波在很多项目中都有用到,但是对于原理却很少有详细分析,而只是直接应用,在看完b站up主DR_CAN视频推导后自行推导一遍和查看其他资料后进行总结,将从最初的递归算法,利用数据融合,协方差矩阵, ...

卡尔曼滤波原理(一)

卡尔曼滤波原理(一)相关推荐

最新文章

热门文章