%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 扩展Kalman滤波实现三维位置估计
% 观测有距离、俯仰角、偏航角
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function ekf_for_track_9
clc;
clear;
n=9;
T=0.5; % 雷达扫描周期
N=50; % 总的采样次数
F=[1,0,0,T,0,0,T^2/2,0,0;
0,1,0,0,T,0,0,T^2/2,0;
0,0,1,0,0,T,0,0,T^2/2;
0,0,0,1,0,0,T,0,0;
0,0,0,0,1,0,0,T,0;
0,0,0,0,0,1,0,0,T;
0,0,0,0,0,0,1,0,0;
0,0,0,0,0,0,0,1,0;
0,0,0,0,0,0,0,0,1]; % 状态转移矩阵
Q=[1 0 0 0 0 0 0 0 0; % 过程噪声协方差矩阵
0 1 0 0 0 0 0 0 0;
0 0 1 0 0 0 0 0 0;
0 0 0 0.01 0 0 0 0 0;
0 0 0 0 0.01 0 0 0 0;
0 0 0 0 0 0.01 0 0 0;
0 0 0 0 0 0 0.0001 0 0;
0 0 0 0 0 0 0 0.0001 0;
0 0 0 0 0 0 0 0 0.0001];
% R=0.1*pi/180; % 观测噪声方差（因为只有一个观测，所以是一个值，不是协方差矩阵）
R = [100 0 0; % 观测噪声协方差矩阵
0 0.001^2 0
0 0 0.001^2];
X=zeros(9,N); % 目标真实位置、速度
X(:,1)=[1000;5000;200;10;50;10;2;-4;2]+sqrtm(Q)*randn(n,1);
Z=zeros(3,N); % 传感器对位置的观测
x0=0;
y0=0;
z0=0;
Xstation=[x0;y0;z0]; % 观测站的位置
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for t=2:N
X(:,t)=F*X(:,t-1)+sqrtm(Q)*randn(9,1); % 目标真实轨迹
end
for t=1:N
% Z(t)=hfun(X(:,t),Xstation)+w(t); % 对目标的观测
[dd,alpha,beta]=funh(X(:,t),Xstation);
Z(:,t) = [dd,alpha,beta]' + sqrtm(R)*randn(3,1);
end
% EKF滤波
Xekf=zeros(9,N); % EKF滤波值
Xekf(:,1)=X(:,1); % EKF滤波初始化
P0 =[100 0 0 0 0 0 0 0 0; % 协方差初始化
0 100 0 0 0 0 0 0 0;
0 0 100 0 0 0 0 0 0;
0 0 0 1 0 0 0 0 0;
0 0 0 0 1 0 0 0 0;
0 0 0 0 0 1 0 0 0;
0 0 0 0 0 0 0.1 0 0;
0 0 0 0 0 0 0 0.1 0
0 0 0 0 0 0 0 0 0.1];
for i=2:N
Xn=F*Xekf(:,i-1); % 预测
P1=F*P0*F'+ Q; % 预测误差协方差 没有G就不写
[dd,alpha,beta]=funh(Xn,Xstation); % 观测预测
% 求Jacobian矩阵H，H为3行9列的矩阵
D = Dist(Xn,Xstation);
DD = Dist3(Xn,Xstation);
H = [(Xn(1,1)-x0)/DD,(Xn(2,1)-y0)/DD,(Xn(3,1)-z0)/DD,0,0,0,0,0,0;
-(Xn(2,1)-y0)/D^2,(Xn(1,1)-x0)/D^2,0,0,0,0,0,0,0;
(1/(1+((Xn(3,1)-z0)/D)^2)).*(-2*(Xn(1,1)-x0)/D^4),(1/(1+((Xn(3,1)-z0)/D)^2)).*(-2*(Xn(2,1)-y0)/D^4),(1/D)/(1/(1+((Xn(3,1)-z0)/D)^2)),0,0,0,0,0,0];
K=P1*H'*inv(H*P1*H'+R); % kalman增益
Xekf(:,i)=Xn+K*(Z(:,i)-[dd,alpha,beta]'); % 状态更新
P0=(eye(9)-K*H)*P1; % 协方差更新
end
Err_KalmanFilter = zeros(1,N);
for i=1:N
Err_KalmanFilter(i)= (Dist3(X(:,i),Xekf(:,i)));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 画图，轨迹图
figure
t=1:N;
hold on;
box on;
grid on;
plot3(X(1,t),X(2,t),X(3,t),'k-');
plot3(Z(1,t).*cos(Z(3,t)).*cos(Z(2,t)),Z(1,t).*cos(Z(3,t)).*sin(Z(2,t)),Z(1,t).*sin(Z(3,t)),'-b.');
plot3(Xekf(1,t),Xekf(2,t),Xekf(3,t),'-r.');
legend('实际值','测量值','ekf估计值');
xlabel('x方向位置/米')
ylabel('y方向位置/米')
zlabel('z方向位置/米')
view(3)
figure
hold on;
box on;
plot(Err_KalmanFilter,'-ks','MarkerFace','r');
legend('跟踪误差EKF');
%figure
%hold on;
%box on;
%plot(Z/pi*180,'-r.','MarkerFace','r');
%plot(Z/pi*180+w/pi*180,'-ko','MarkerFace','g');
%legend('真实角度','观测角度');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function cita=hfun(X1,X0)
% 先判断飞机在观测站的哪个方向
if X1(3,1)-X0(2,1)>=0 % 上
if X1(1,1)-X0(1,1)>0 % 右上
cita=atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) ));
elseif X1(1,1)-X0(1,1)==0 % 正上
cita=pi/2;
else % 左上
cita=pi/2+atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) ));
end
else % 下
if X1(1,1)-X0(1,1)>0 % 右下
cita=3*pi/2+atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) ));
elseif X1(1,1)-X0(1,1)==0 % 正下
cita=3*pi/2;
else % 左下
cita=pi+atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) ));
end
end
%%%%%%%%%%%%% 求两点部分距离（三维）Sx2 + Sy2 %%%%%%%%%%%%%%
function D = Dist(X1,X0)
D = sqrt( (X1(1,1)-X0(1,1))^2+(X1(2,1)-X0(2,1))^2 );
%%%%%%%%%%%%% 求两点距离（三维）%%%%%%%%%%%%%%
function DD = Dist3(X1,X0)
DD = sqrt( (X1(1,1)-X0(1,1))^2+(X1(2,1)-X0(2,1))^2+(X1(3,1)-X0(3,1))^2 );
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 观测函数
% 输入：飞机和观测站的坐标
% 输出：dd为飞机到观测站的距离
% alpha为观测站到飞机的偏航角
% beta为观测站到飞机的俯仰角
function [dd,alpha,beta]=funh(X1,X0)
dd = sqrt((X1(1,1)-X0(1,1))^2+(X1(2,1)-X0(2,1))^2+(X1(3,1)-X0(3,1))^2);
alpha = atan((X1(2,1)-X0(2,1))/(X1(1,1)-X0(1,1)));
beta = atan((X1(3,1)-X0(3,1))/sqrt((X1(1,1)-X0(1,1))^2+(X1(2,1)-X0(2,1))^2));
