雷达对远程目标的距离测量、声呐对潜伏在海底的潜艇的距离测量、温度计对温度的测量等都属于一维信号测量。以激光测距仪对架空线的高度测量为例,编程实现 Kalman 滤波。
假定测距仪的采样时间是 10ms,仿真时间是1s。架空线受到风的干扰。过程噪声均值为0,方差为Q=0.0001;初始状态X(0)的值认为就是第一次测距仪测量值8.01m。测距仪的测量噪声均值为0,噪声方差为R=0.0025。架空线的状态方程为;
X
(
k
)
=
Φ
X
(
k
−
1
)
+
T
W
(
k
)
X(k)=\Phi X(k-1)+\Tau W(k)
X(k)=ΦX(k−1)+TW(k)
测距仪的测量方程为∶
Z
(
k
)
=
H
X
(
k
−
1
)
+
V
(
k
)
Z(k)=HX(k-1)+V(k)
Z(k)=HX(k−1)+V(k)
式中,
Φ
=
1
\Phi=1
Φ=1,
T
=
1
\Tau=1
T=1,
H
=
1
H=1
H=1 。W和V为过程噪声和观测噪声。那么编写MATLAB程序仿真Kalman 滤波应用的过程如下。
%%function LinerKalman1D %一维Kalman滤波应用仿真
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%系统信息
F= 1;%状态转移矩阵
G=1;%噪声驱动矩阵
H=1;%观测矩阵
Q= 0.0001;%过程噪声方差
R= 0.0025;%观测噪声方差
T=100;%仿真总步数
W = sqrt( Q) * randn( 1 ,T);%各时刻的过程噪声
V = sqrt( R) * randn( 1,T);%各时刻的观测噪声
I= eye( 1 ) ;%单位1矩阵
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 开始仿真目标状态和测量过程
X = zeros( 1,T);X(1,1)= 8.00; %状态初始化,也叫目标真实值
Z= zeros( 1 ,T);Z( 1,1)=8.01; %观测初始化
Xkf= zeros( 1,T);Xkf( 1 ,1)=Z( 1,1);% Kalman 滤波得到的状态初始化
P=zeros( 1,T);P( 1,1)= 0.0001;%协方差初始化
for k=2:T
%被测量目标一侧信息
%X是架空线的真实高度值,它由真实值和风吹扰动导致的干扰组成
%X是计算机模拟仿真,是真实状态的模拟,测距仪是永远测不到的
X( 1,k)=F*X( 1,k-1)+G* W( 1,k);%状态方程
%% 卡尔曼滤波的主要步骤
%观测站一侧的信息
%测距仪只能通过传感器测量,测量信息是Z,根据测量信息开始滤波
Z( 1 ,k)=H* X( 1,k)+V( 1,k ) ; %。观测方程
Xpre=H * Xkf( 1 ,k-1 ) ; %第一步:状态预测
Ppre=F*P(k- 1 ) *F+Q; %第二步:协方差预测
K=Ppre * inv(H * Ppre * H'+R) ; %第三步:计算Kalman增益
e=Z( k)-H * Xpre ; %计算新息
Xkf( 1, k ) = Xpre+K * e ; %。第四步:状态更新
P( 1,k)= (I-K* H) * Ppre ; %。第五步:协方差更新
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 误差分析
MeasuredDeviation = abs(Z-X);
FilterResultDeviation = abs ( Xkf-X);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%画图
figure %轨迹图
hold on ; box on ; axis([ 0 T 7.5 8.5]) %画图框架初始化
xlabel ( 'time/10ms') ; ylabel ( 'messure value' ) ; % x、y轴名称
plot( 1:T,X( 1,1:T) , '-k') ; %真实值
plot( 1 :T,Z( 1,1 :T) , '-ko' , 'MarkerFaceColor' , 'g') ; %测量值
plot( 1 : T,Xkf( 1 : T),'-r.' ) ; %滤波值
legend ( 'true value' , 'measured value' , 'filtered value' ) ;
figure %误差分析图
hold on; box on ; %axis( [ 0 T 7.5 8.5])
%画图框架初始化
xlabel ( 'time/10ms') ; ylabel( 'value of the deviation') ;% x\ y轴名称
plot ( MeasuredDeviation , '-r.' , 'MarkerFaceColor' , 'g');%测量误差
plot ( FilterResultDeviation , '-ko' , 'MarkerFaceColor' ,'g'); % 滤波结果误差
legend( 'measured deviation' , ' filtered deviation') ;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %