www.pudn.com > strong-tracking-filter.rar > cs_kalman_filter_3D.m, change:2010-10-09,size:1315b


function [x1,P1,r,S]=cs_kalman_filter_3D(x0,P0,F,U,H,R,T,alpha1,alpha2,alpha3,... 
                                  a_max1,a_max_minus1,a_max2,a_max_minus2,a_max3,a_max_minus3,z) 
% 同时考虑三个变量的CS模型对应的kalman滤波器 
% a_max:加速度上限 
% a_max_minus:加速度下限 
fake_x_pro=F*x0;        % 伪一步预估值,只为了得到加速度分量的估计值 
avg_a1=fake_x_pro(3,1); 
avg_a2=fake_x_pro(6,1); 
avg_a3=fake_x_pro(9,1); 
if (avg_a1>=0) 
    sigma1=((4-pi)/pi)*(a_max1-avg_a1)^2; 
else 
    sigma1=((4-pi)/pi)*(a_max_minus1+avg_a1)^2; 
end 
if (avg_a2>=0) 
    sigma2=((4-pi)/pi)*(a_max2-avg_a2)^2; 
else 
    sigma2=((4-pi)/pi)*(a_max_minus2+avg_a2)^2; 
end 
if (avg_a3>=0) 
    sigma3=((4-pi)/pi)*(a_max3-avg_a3)^2; 
else 
    sigma3=((4-pi)/pi)*(a_max_minus3+avg_a3)^2; 
end 
Q1=basic_Q(alpha1,T); 
Q2=basic_Q(alpha2,T); 
Q3=basic_Q(alpha3,T); 
Q=zeros(9,9); 
Q(1:3,1:3)=2*alpha1*sigma1*Q1; 
Q(4:6,4:6)=2*alpha2*sigma2*Q2; 
Q(7:9,7:9)=2*alpha3*sigma3*Q3; 
avg_a=[avg_a1,avg_a2,avg_a3]'; 
x_pro=F*x0+U*avg_a;        % 状态一步预估值 
P_pro=F*P0*F'+Q;             % 协方差一步预估值 
r=z-H*x_pro;                        % 残差(新息) 
S=H*P_pro*H'+R;               % 新息协方差 
K=P_pro*H'*inv(S);             % 增益 
x1=x_pro+K*r;                     % 状态更新方程 
P1=P_pro-K*S*K';              % 协方差更新方程