kalman.txt

作为用MATLAB语言编写的一段卡尔曼滤波程序

offtime=800;
Dm=0.00004;
a=0.05;
[Os,Ms,Ns]=trajectory(Dm,offtime);&产生航轨迹
Xk=[Os;Ms;Ns]; % X(k)
Zk=zeros(3,1); % Z(k)
Xest=zeros(3,1); % 用前k-1时刻的输出值估计k时刻的预测值:X^(k+1/k)
Xfli=zeros(3,1);  %k时刻Kalman滤波器的输出值:X^(k/k)
Pxe=zeros(3,1); % 预测输出误差均方差矩阵:P(k+1/k)
Px=zeros(3,1);  %滤波输出误差均方差矩阵:P(k/k)
XE=zeros(3,234); % 得到最终的滤波输出值
W=0.1*randn(1,length(Os));
R=cov(W);
for k=1:length(Os)
  Ts(k)=ceil(sin(k)+2);
       if Ts(k)<2
           Ts(k)=1; %产生不等间隔采样
       end
%定义kf参数
Phi=[1,Ts(k),(a*Ts(k)-1+exp(-a*Ts(k))/(a^2));0,1,(1-exp(-a*Ts(k)))/a;0,0,exp(-a*Ts(k))];
H=[1,0,0];
Q(1,1)=Dm^2/a^4*(1-exp(-2*a*Ts(k))+2*a*Ts(k)+2*(a*Ts(k))^3/3-2*(a*Ts(k))^2-4*a*Ts(k)*exp(-a*Ts(k)));
Q(1,2)=Dm^2/a^3*(exp(-2*a*Ts(k))+1-2*exp(-a*Ts(k))+2*a*Ts(k)*exp(-a*Ts(k))-2*a*Ts(k)+(a*Ts(k))^2);
Q(1,3)=Dm^2/a^2*(1-exp(-2*a*Ts(k))-2*a*Ts(k)*exp(-a*Ts(k)));
Q(2,1)=Q(1,2);
Q(2,2)=Dm^2/a^2*(4*exp(-a*Ts(k))-3-exp(-2*a*Ts(k))+2*a*Ts(k));
Q(2,3)=Dm^2/a*(exp(-2*a*Ts(k))+1-2*exp(-a*Ts(k)));
Q(3,1)=Q(1,3);
Q(3,2)=Q(2,3);
Q(3,3)=Dm^2*(1-exp(-2*a*Ts(k)));
Zk=H*Xk+W; % 测量方程 
end
Xfli=[Zk(2),(Zk(2)-Zk(1))/Ts(k),0]'; %利用前两个观测值来对初始条件进行估计
Px=[R,R/Ts(k),0;R/Ts(k),(2*R)/((Ts(k))^2),0;0,0,0];
for K=1:length(Os)
        Pxe=Phi*Px*Phi'+Q; % 预测误差的协方差阵
        K=Pxe*H'*inv(H*Pxe*H'+R); % Kalman滤波增益
        Px=(eye(3)-K*H)*Pxe; %协方差更新
        Xest=Phi*Xfli; % 预测该时刻的状态
        Xfli=Xest+K*(Zk(k)-H*Xest); %状态更新
        O(k)=Xfli(1,1);
        M(k)=Xfli(2,1);
        N(k)=Xfli(3,1);
end
     XE=[O;M;N];滤波输出