www.gusucode.com > 目标跟踪问题的应用背景是雷达数据处理matlab源码程序 > KalmanFilterProgram/Kalman_filter.m
function XE=Kalman_filter(Ts,offtime,d,Flag) % Kalman_filter 采用Kalman滤波方法,从观测数值中得到航迹的估计 % XE 输出x轴方向上的误差 % Ts 采样时间,即雷达工作周期 % offtime 仿真截止时间 % d 噪声的标准差值 % Flag 判断计算x轴或y轴数据,'0'--x,'1'--y if nargin>4 error('输入的变量过多,请检查'); end if offtime<600 error('仿真时间必须大于600s,请重新输入'); end Pv=d*d; % 噪声的功率 N=ceil(offtime/Ts); % 采样点数 sigma=10;% 加速度方向的的扰动 switch Flag case 0 a=[zeros(1,400) 0.075*ones(1,200) zeros(1,10) -0.3*ones(1,50) zeros(1,offtime-660)]; % 对不同时段的加速度进行描述 case 1 a=[zeros(1,400) 0.075*ones(1,200) zeros(1,10) 0.3*ones(1,50) zeros(1,offtime-660)]; otherwise error('输入仅能为0或1'); end % 定义系统的状态方程 Phi=[1,Ts;0,1]; Gamma=[Ts*Ts/2;Ts]; C=[1 0]; R=Pv; Q=sigma^2;W=[]; randn('state',sum(100*clock)); % 设置随机数发生器 for n=0:Ts:offtime-1 W(n/Ts+1)=a(n+1)+sigma*randn(1,1); end Xest=zeros(2,1); % 用前k-1时刻的输出值估计k时刻的预测值 Xfli=zeros(2,1); % k时刻Kalman滤波器的输出值 Xes=zeros(2,1); % 预测输出误差 Xef=zeros(2,1); % 滤波后输出的误差 Pxe=zeros(2,1); % 预测输出误差均方差矩阵 Px=zeros(2,1); % 滤波输出误差均方差矩阵 XE=zeros(1,N); % 得到最终的滤波输出值,仅仅考虑距离分量 [x,y]=trajectory(Ts,offtime); % 产生理论的航迹 for i=1:N vx(i)=d*randn(1); % 观测噪声,两者独立 vy(i)=d*randn(1); zx(i)=x(i)+vx(i); % 实际观测值 zy(i)=y(i)+vy(i); end switch Flag case 0 Xfli=[zx(2) (zx(2)-zx(1))/Ts]'; %利用前两个观测值来对初始条件进行估计 Xef=[-vx(2) Ts*W(1)/2+(vx(1)-vx(2))/Ts]'; Px=[Pv,Pv/Ts;Pv/Ts,2*Pv/Ts+Ts*Ts*Q/4]; for k=3:N Xest=Phi*Xfli; % 更新该时刻的预测值 Xes=Phi*Xef+Gamma*W(k-1); % 预测输出误差 Pxe=Phi*Px*Phi'+Gamma*Q*Gamma'; % 预测误差的协方差阵 K=Pxe*C'*inv(C*Pxe*C'+R); % Kalman滤波增益 Xfli=Xest+K*(zx(k)-C*Xest); Xef=(eye(2)-K*C)*Xes-K*vx(k); Px=(eye(2)-K*C)*Pxe; XE(k)=Xfli(1,1); end XE(1)=zx(1);XE(2)=zx(2); case 1 Xfli=[zy(2) (zy(2)-zy(1))/Ts]'; %利用前两个观测值来对初始条件进行估计 Xef=[-vy(2) Ts*W(1)/2+(vy(1)-vy(2))/Ts]'; Px=[Pv,Pv/Ts;Pv/Ts,2*Pv/Ts+Ts*Ts*Q/4]; for k=3:N Xest=Phi*Xfli; % 更新该时刻的预测值 Xes=Phi*Xef+Gamma*W(k-1); % 预测输出误差 Pxe=Phi*Px*Phi'+Gamma*Q*Gamma'; % 预测误差的协方差阵 K=Pxe*C'*inv(C*Pxe*C'+R); % Kalman滤波增益 Xfli=Xest+K*(zy(k)-C*Xest); Xef=(eye(2)-K*C)*Xes-K*vy(k); Px=(eye(2)-K*C)*Pxe; XE(k)=Xfli(1,1); end XE(1)=zy(1);XE(2)=zy(2); otherwise error('False iuput nargin'); end