扩展Kalman滤波(EKF)和无迹卡尔曼滤波 PPT

X(k,:) = [x, vx, y, vy];
end
figure(1), hold off, plot(X(:,1),X(:,3),'-b'), grid on
% figure(2), plot(X(:,2:2:4))
% 构造量测量
mrad = 0.001;
dr = 10; dafa = 10*mrad; % 量测噪声
三、无迹卡尔曼滤波算法(UKF)
三、无迹卡尔曼滤波算法(UKF)
扩展Kalman滤波(EKF)和无迹卡尔曼滤波
一、背景
普通卡尔曼滤波是在线性高斯情况下利用最小均方误差准则获得 目标的动态估计，适应于过程和测量都属于线性系统， 且误差符 合高斯分布的系统。 但是实际上很多系统都存在一定的非线性， 表现在过程方程 （状态方程）是非线性的，或者观测与状态之间 的关系（测量方程）是非线性的。这种情况下就不能使用一般的卡 尔曼滤波了。解决的方法是将非线性关系进行线性近似，将其转化 成线性问题。 对于非线性问题线性化常用的两大途径： （1） 将非线性环节线性化，对高阶项采用忽略或逼近措施；（EKF） （2）用采样方法近似非线性分布. ( UKF)
二、扩展Kalman滤波(EKF)算法
假定定位跟踪问题的非线性状态方程和测量方程如 下:
Xk1f(Xk)W k ...........1 ..)..(
Ykh(Xk)Vk ...............(.2.)...
在最近一次状态估计的时刻，对以上两式进
行线性化处理，首先构造如下2个矩阵:
F(k1k)f (Xk) X
2n
Py wic (Yi y)(Yi y)T i0
..............9( )
2n
Pxy wic (Yi x)(Yi y)T i0
............(10)
三、无迹卡尔曼滤波算法(UKF)
UKF是用确定的采样来近似状态的后验PDF,可以 有效解决由系统非线性的加剧而引起的滤波发散问 题，但UKF仍是用高斯分布来逼近系统状态的后验概 率密度，所以在系统状态的后验概率密度是非高斯 的情况下，滤波结果将有极大的误差。
F(3,4) = 1;
F(4,4) = 2*ky*vy;
2) = 1;
二、扩展Kalman滤波(EKF)算法
二、扩展Kalman滤波(EKF)算法
二、扩展Kalman滤波(EKF)算法
Y
ekf simulation
520
real
measurement
500
ekf estimated
480
460
(1)计算2n+ 1个Sigma点及其权值:
X0 x,Xi x (n1)Px Xi x (n1)Px
i 1,2,...,n i n1,...2, n ......5()
ww00cm//((nn))(12 ) wim wic 1/2(n), i 1,2,...2, n
..................6.()
%%%%%%%%%%%%%%%%%%%%子程序%%%%%%%%%%%%%%%%%%%
function F = JacobianF(X, kx, ky, g) % 系统状态雅可比函数
vx = X(2); vy = X(4);
F = zeros(4,4);
F(1, F(2,2) = -2*kx*vx;
440
420
400
380
360
0
20
40 60
80 100 120 140 160 180 200
X
图2 仿真结果
三、无迹卡尔曼滤波算法(UKF)
为了改善对非线性问题进行滤波的效果， Julier 等人提出了采用基于unscented变换的UKF方 法UKF不是和EKF一样去近似非线性模型，而是对后 验概率密度进行近似来得到次优的滤波算法。
XX(k k)
.......3()
H(K) h(Xk ) X
XX(k k1)
..........(.4)
二、扩展Kalman滤波(EKF)算法
二、扩展Kalman滤波(EKF)算法
二、扩展Kalman滤波(EKF)算法
vy = vy + (ky*vy^2-g+day*randn(1))*Ts;
UKF算法的核心是UT变换，UT是一种计算非线性 变换中的随机变量的统计特征的新方法，是UKF的基 础。
三、无迹卡尔曼滤波算法(UKF)
假设n维随机向量x: N(x,Px),x通过非线性函数y=f(x) 变换后得到n维的随机变量y。通过UT变换可以以较 高的精度和较低的计算复杂度求得y的均值 y 和方 差 Px 。UT的具体过程可描述如下:
for k=1:len
r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1);
a = atan(X(k,1)/X(k,3)) + dafa*randn(1,1);
ቤተ መጻሕፍቲ ባይዱ
Z(k,:) = [r, a];
end
大家应该也有点累了，稍作休息
大家有疑问的，可以询问和交流
二、扩展Kalman滤波(EKF)算法
二、扩展Kalman滤波(EKF)算法
EKF算法是一种近似方法，它将非线性模型在状态 估计值附近作泰勒级数展开，并在一阶截断，用得 到的一阶近似项作为原状态方程和测量方程近似表 达形式，从而实现线性化同时假定线性化后的状态 依然服从高斯分布，然后对线性化后的系统采用标 准卡尔曼滤波获得状态估计。采用局部线性化技术， 能得到问题局部最优解，但它能否收敛于全局最优 解，取决于函数的非线性强度以及展开点的选择。
三、无迹卡尔曼滤波算法(UKF)
2(nk)n
(n)Px
w
m i
w
c i
f (•)
Y if(X i)i,0 ,1 ,.2 .n.,.......7 ).(....
三、无迹卡尔曼滤波算法(UKF)
2n
y wimYi .................................(.8) i0
二、扩展Kalman滤波(EKF)算法
figure(1), plot(X_est(:,1),X_est(:,3), '+r')
xlabel('X'); ylabel('Y'); title('ekf simulation');
legend('real', 'measurement', 'ekf estimated');