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