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


function [x1,P1,r,S]=sg_kalman_filter_3D(x0,P0,F,U,H,R,T,alpha1,alpha2,alpha3,... 
                                  a_max1,p1,a_max2,p2,a_max3,p3,p0,z) 
% 同时考虑三个变量的CS模型对应的kalman滤波器 
% a_max:加速度上限 
% a_max_minus:加速度下限 
delta1=a_max1^2*(1+4*p1-p0)/3; 
delta2=a_max2^2*(1+4*p2-p0)/3; 
delta3=a_max3^2*(1+4*p3-p0)/3; 
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*delta1*Q1; 
Q(4:6,4:6)=2*alpha2*delta2*Q2; 
Q(7:9,7:9)=2*alpha3*delta3*Q3; 
x_pro=F*x0;        % 状态一步预估值 
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';              % 协方差更新方程