function GPSDREKF
clear all;
clc;
%初始化
T=1;hits=500; % 采样周期，采样点数
xe=0;ve=10;ae=0;xn=0;vn=10;an=0; % 状态向量初值
X0=[xe ve ae xn vn an]';
P0=diag([100,1,0.04,100,1,0.04]); % 估计误差协方差阵初值
e_Q=16;n_Q=15;w_Q=0.005;s_Q=0.7; % 观测噪声标准差
re=1;rn=1;
O3=zeros(3,3);
% 模型噪声协方差阵
q1=0.5*re^(-5)*(1-exp(-2*re*T)+2*re*T+2*re^3*T^3*3^(-1)-2*re^2*T^2-4*re*T*exp(-re*T));
q2=0.5*re^(-4)*(1+exp(-2*re*T)-2*exp(-re*T)-2*re*T+re^2*T^2+2*re*T*exp(-re*T));
q3=0.5*re^(-3)*(1-exp(-2*re*T)-2*re*T*exp(-re*T));
q4=q2;
q5=0.5*re^(-3)*(-3-exp(-2*re*T)+4*exp(-re*T)+2*re*T);
q6=0.5*re^(-2)*(1+exp(-2*re*T)-2*exp(-re*T));
q7=q3;
q8=q6;
q9=0.5*re^(-1)*(1-exp(-2*re*T));
Qe=[q1 q2 q3;q4 q5 q6;q7 q8 q9];
Qn=Qe;
Q=[2*Qe O3;O3 2*Qn];
R=diag([e_Q^2,n_Q^2,w_Q^2,s_Q^2]); % 观测噪声协方差阵
% 模拟观测向量（滤波器输入信号）
Fe=[1 T (T^2)/2;0 1 T;0 0 1]; % 状态转移矩阵
Fn=Fe;
F=[Fe O3;O3 Fn];
ae_Q=0.3;an_Q=0.3; % 模型噪声
noise_ae=ae_Q*randn(1,hits);
noise_an=an_Q*randn(1,hits);
Ohits=zeros(1,hits);
W=[Ohits;Ohits;noise_ae;Ohits;Ohits;noise_an];
X=zeros(6,hits);
for t=1:T:hits % 状态向量
if(t==1)
%X(:,t)=X0+W(:,t);
X(:,t)=X0;
else
X(:,t)=F*X(:,t-1)+W(:,t-1);
end
end
eobs=X(1,:);nobs=X(4,:); % 模型输出
w=(X(5,:).*X(3,:)-X(2,:).*X(6,:))./(X(5,:).^2+X(2,:).^2);
s=T*sqrt(X(5,:).^2+X(2,:).^2);
Z=[eobs;nobs;w;s];
noise_e=e_Q*randn(1,hits); % 观测噪声
noise_n=n_Q*randn(1,hits);
noise_w=w_Q*randn(1,hits);
noise_s=s_Q*randn(1,hits);
Z_T=Z+[noise_e;noise_n;noise_w;noise_s]; % 观测向量
X0=[0 10 0 0 10 0]';
Xk=kalmanf(X0,P0,Z_T,T,R,Q); % kalman滤波
Dxe=X(1,:)-Xk(1,:); % 东向的滤波误差
Dxn=X(4,:)-Xk(4,:); % 北向的滤波误差
De=sqrt(cov(Dxe)); % 滤波误差方差
Dn=sqrt(cov(Dxn));
t=1:hits;
figure(1)
subplot(2,1,1);
plot(t,Dxe(t),'r',t,Z_T(1,:)-X(1,:),'g') ,grid % 滤波前后东向误差曲线比较
xlabel('t/s')
ylabel('error/m')
title('东向')
% axis([0 hits -50 50])
legend('滤波前误差','滤波后误差')
subplot(2,1,2);
plot(t,Dxn(t),'r',t,Z_T(2,:)-X(4,:),'g') ,grid % 滤波前后北向误差曲线比较
xlabel('t/s')
ylabel('error/m')
title('北向')
% axis([0 hits -50 50])
legend('滤波前误差','滤波后误差')
figure(2)
plot(X(1,:),X(4,:),'r',Xk(1,:),Xk(4,:),'g')
legend('真实轨迹','估计轨迹')
function f=kalmanf(X0,P0,Z_T,T,R,Q)
hits=size(Z_T,2);
Fe=[1 T (T^2)/2;0 1 T;0 0 1]; % 状态转移矩阵
Fn=Fe;
O3=zeros(3,3);
F=[Fe O3;O3 Fn];
for t=1:T:hits
Xk_predict=F*X0; % 预测方程
x1=Xk_predict(4);
x2=Xk_predict(1);
v1=Xk_predict(5);
v2=Xk_predict(2);
a1=Xk_predict(6);
a2=Xk_predict(3);
h1=(a1*v2-2*v2*v1*a2-a1*v1^2)/((v1^2+v2^2)^2); % 测量方程在
% 预测点的Jacobi矩阵
h2=v1/(v1^2+v2^2);
h3=(a2*v1+2*v2*v1*a1-a2*v1^2)/((v1^2+v2^2)^2);
h4=-v2/(v1^2+v2^2);
h5=T*v2/sqrt(v1^2+v2^2);
h6=T*v1/sqrt(v1^2+v2^2);
H=[1 0 0 0 0 0 ;0 0 0 1 0 0 ;0 h1 h2 0 h3 h4;0 h5 0 0 h6 0];
Pk_predict=F*P0*F'+Q; % 预测误差协方差阵
S=H*Pk_predict*H'+R; % 信息协方差阵
K=Pk_predict*H'/S; % 增益矩阵
Z_predict=[x2 x1 (v1*a2-v2*a1)/(v1^2+v2^2) T*sqrt(v1^2+v2^2)]';
Xk(:,t)=Xk_predict+K*(Z_T(:,t)-Z_predict); % 估计矩阵(最后的输出值)
Pk=(eye(6)-K*H)*Pk_predict; % 估计误差协方差阵
P0=Pk; % 估计误差协方差阵
X0=Xk(:,t); % 估计矩阵更新
end
f=Xk;
