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';              % 协方差更新方程

```