EKF matlab

最新推荐文章于 2025-05-10 15:34:29 发布

学之之博未若知之之要知之之要未若行之之实

最新推荐文章于 2025-05-10 15:34:29 发布

阅读量5.6k

点赞数 3

卡尔曼是真的强，我只能这么说，前面说了卡尔曼滤波，它是针对线性系统的滤波方法。但在实际工作中，我们的系统几乎都是非线性的，所以如何解决非线性系统的滤波问题呢，这就是我们要讲的EKF（扩展卡尔曼滤波），它的原理很简单，就是在估计状态的地方进行线性化，线性化的方法也很简单，只需要进行泰勒的一阶展开就行。当然我们也要说一下缺点，因为我们选择的线性化方法，所以只能达到二阶的精度（UKF可以达到四阶的精度），所以要求我们的系统是弱线性化的。其实UKF也是对非线性系统的卡尔曼滤波，主要本人对其线性化方法（UT变换）不是很理解，等考完试再说。

首先，系统的状态方程和观测方程如下：
${\textbf {x}}_{{k}}=f({\textbf {x}}_{{k-1}},{\textbf {u}}_{{k}},{\textbf {w}}_{{k}})$
${\textbf {z}}_{{k}}=h({\textbf {x}}_{{k}},{\textbf {v}}_{{k}})$
我们知道，在更新误差协方差矩阵的时候，不能直接用f和h的，所以我们要进行泰勒展开，也就是要求雅克比矩阵。再利用线性情况下的卡尔曼滤波进行计算更新。
预测：
${\hat {{\textbf {x}}}}_{{k|k-1}}=f({\textbf {x}}_{{k-1}},{\textbf {u}}_{{k}},0)$
${\textbf {P}}_{{k|k-1}}={\textbf {F}}_{{k}}{\textbf {P}}_{{k-1|k-1}}{\textbf {F}}_{{k}}^{{T}}+{\textbf {Q}}_{{k}}$
利用雅克比矩阵进行更新模型：

${\textbf {F}}_{{k}}=\left.{\frac {\partial f}{\partial {\textbf {x}}}}\right\vert _{{{\hat {{\textbf {x}}}}_{{k-1|k-1}},{\textbf {u}}_{{k}}}}$
${\textbf {H}}_{{k}}=\left.{\frac {\partial h}{\partial {\textbf {x}}}}\right\vert _{{{\hat {{\textbf {x}}}}_{{k|k-1}}}}$
更新：
${\tilde {{\textbf {y}}}}_{{k}}={\textbf {z}}_{{k}}-h({\hat {{\textbf {x}}}}_{{k|k-1}},0)$
${\textbf {S}}_{{k}}={\textbf {H}}_{{k}}{\textbf {P}}_{{k|k-1}}{\textbf {H}}_{{k}}^{{T}}+{\textbf {R}}_{{k}}$
${\textbf {K}}_{{k}}={\textbf {P}}_{{k|k-1}}{\textbf {H}}_{{k}}^{{T}}{\textbf {S}}_{{k}}^{{-1}}$
${\hat {{\textbf {x}}}}_{{k|k}}={\hat {{\textbf {x}}}}_{{k|k-1}}+{\textbf {K}}_{{k}}{\tilde {{\textbf {y}}}}_{{k}}$
${\textbf {P}}_{{k|k}}=(I-{\textbf {K}}_{{k}}{\textbf {H}}_{{k}}){\textbf {P}}_{{k|k-1}}$
下面我会展示一个目标追踪的EKF例子：

12/05/16 01:22:15 /home/fc/桌面/EKF/EKF.m

1	% 扩展Kalman滤波在目标跟踪中的应用
2	% functionEKF_For_TargetTracking
3	clc ; clear ;
4	T = 1 ; % 雷达扫描周期
5	N = 60 / T ; % 总的采样次数
6	X = zeros ( 4 , N ) ; % 目标真实位置、速度
7	X ( : , 1 ) = [ - 100 , 2 , 200 , 20 ] ; % 目标初始位置、速度
8	Z = zeros ( 1 , N ) ; % 传感器对位置的观测
9	delta_w = 1e-3 ; % 如果增大这个参数，目标的真实轨迹就是曲线了
10	Q = delta_w * diag ( [ 0.5 , 1 ] ) ; % 过程噪声方差
11	G = [ T ^ 2 / 2 , 0 ; T , 0 ; 0 , T ^ 2 / 2 ; 0 , T ] ; % 过程噪声驱动矩阵
12	R = 5 ; % 观测噪声方差
13	F = [ 1 , T , 0 , 0 ; 0 , 1 , 0 , 0 ; 0 , 0 , 1 , T ; 0 , 0 , 0 , 1 ] ; % 状态转移矩阵
14	x0 = 200 ; % 观测站的位置
15	y0 = 300 ;
16	Xstation = [ x0 , y0 ] ;
17	for t = 2 : N
18	X ( : , t ) = F * X ( : , t - 1 ) + G * sqrtm ( Q ) * randn ( 2 , 1 ) ; % 目标真实轨迹
19	end
20
21	for t = 1 : N
22	Z ( t ) = Dist ( X ( : , t ) , Xstation ) + sqrtm ( R ) * randn ; % 观测值
23	end
24	% EKF
25	Xekf = zeros ( 4 , N ) ;
26	Xekf ( : , 1 ) = X ( : , 1 ) ; % Kalman滤波的状态初始化
27	P0 = eye ( 4 ) ; % 误差协方差矩阵的初始化
28	for i = 2 : N
29	Xn = F * Xekf ( : , i - 1 ) ; % 一步预测
30	P1 = F * P0 * F ' +GQG ' ; % 预测误差协方差
31	dd = Dist ( Xn , Xstation ) ; % 观测预测
32	% 求解雅克比矩阵H
33	H = [ ( Xn ( 1 , 1 ) - x0 ) / dd , 0 , ( Xn ( 3 , 1 ) - y0 ) / dd , 0 ] ; % 泰勒展开的一阶近似
34	K = P1 * H ' inv(HP1*H ' + R ) ; % 卡尔曼最优增益
35	Xekf ( : , i ) = Xn + K * ( Z ( : , i ) - dd ) ; % 状态更新
36	P0 = ( eye ( 4 ) - K * H ) * P1 ; % 滤波误差协方差更新
37	end
38	% 误差分析
39	for i = 1 : N
40	Err_KalmanFilter ( i ) = Dist ( X ( : , i ) , Xekf ( : , i ) ) ; %
41	end
42	% 画图
43	figure
44	hold on ; boxon ;
45	plot ( X ( 1 , : ) , X ( 3 , : ) , ' -k ' ) ; % 真实轨迹
46	plot ( Xekf ( 1 , : ) , Xekf ( 3 , : ) , ' -r ' ) ; % 扩展Kalman滤波轨迹
47	legend ( ' real trajectory ' , ' EKFtrajectory ' ) ;
48	xlabel ( ' X-axis X/m ' ) ;
49	ylabel ( ' Y-axisY/m ' ) ;
50
51	figure
52	hold on ; boxon ;
53	plot ( Err_KalmanFilter , ' -ks ' , ' MarkerFace ' , ' r ' )
54	xlabel ( ' Time/s ' ) ;
55	ylabel ( ' Positionestimation bias /m ' ) ;
56
57	% 子函数求欧氏距离
58	function d = Dist ( X1 , X2 ) ;
59	if length ( X2 ) <= 2
60	d = sqrt ( ( X1 ( 1 ) - X2 ( 1 ) ) ^ 2 + ( X1 ( 3 ) - X2 ( 2 ) ) ^ 2 ) ;
61	else
62	d = sqrt ( ( X1 ( 1 ) - X2 ( 1 ) ) ^ 2 + ( X1 ( 3 ) - X2 ( 3 ) ) ^ 2 ) ;
63	end
64
65	% 子函数求欧氏距离
66	function d = Dist ( X1 , X2 ) ;
67	if length ( X2 ) <= 2
68	d = sqrt ( ( X1 ( 1 ) - X2 ( 1 ) ) ^ 2 + ( X1 ( 3 ) - X2 ( 2 ) ) ^ 2 ) ;
69	else
70	d = sqrt ( ( X1 ( 1 ) - X2 ( 1 ) ) ^ 2 + ( X1 ( 3 ) - X2 ( 3 ) ) ^ 2 ) ;
71	end
72
73
74
75
76
77
78