加载中…
个人资料
GreenSim团队
GreenSim团队
  • 博客等级:
  • 博客积分:0
  • 博客访问:448,622
  • 关注人气:680
  • 获赠金笔:0支
  • 赠出金笔:0支
  • 荣誉徽章:
正文 字体大小:

三维TDOA_AOA定位的扩展卡尔曼滤波仿真主程序

(2011-07-26 00:05:13)
标签:

杂谈

三维TDOA_AOA定位的扩展卡尔曼滤波仿真主程序

三维TDOA_AOA定位的扩展卡尔曼滤波仿真主程序

三维TDOA_AOA定位的扩展卡尔曼滤波仿真主程序

三维TDOA_AOA定位的扩展卡尔曼滤波仿真主程序

%% 三维TDOA/AOA定位的扩展卡尔曼滤波仿真主程序
clc
clear
close all
%% 设置观测站分布
N=4;%观测站个数
%主站位于坐标原点
x1=0;
y1=0;
z1=0;
x2=1000;
y2=0;
z2=0;
x3=0;
y3=1000;
z3=0;
x4=1000;
y4=1000;
z4=0;
XI=[x1,x2,x3,x4];
YI=[y1,y2,y3,y4];
ZI=[z1,z2,z3,z4];
%% 产生观测数据
%假设被定位对象匀速地从A点(100,300,300)运动到B点(900,700,0),单位:米
Px=[100:0.8:900]';
Py=[300:0.4:700]';
Pz=[300:-0.3:0]';
%假设采样时间间隔为1s
T=1;
%根据观测误差水平,产生各个观测站的观测量
M=length(Px);%离散点数
Alpha=zeros(M,N);
Beta=zeros(M,N);
Distance=zeros(M,N);
Sigma_Alpha_Theta=1;%以角度形式表示的角度噪声标准差
Sigma_Alpha=((Sigma_Alpha_Theta)/180)*pi;%转化为弧度表示的角度噪声方差
Sigma_Beta_Theta=1.5;%以角度形式表示的角度噪声标准差
Sigma_Beta=((Sigma_Beta_Theta)/180)*pi;%转化为弧度表示的角度噪声方差
Sigma_Distance=2;%距离测量的标准差
for n=1:N
    Alpha(:,n)=atan2(Py-YI(n),Px-XI(n))+Sigma_Alpha*randn(M,1);
    Beta(:,n)=atan2(Pz-ZI(n),sqrt((Px-XI(n)).^2+(Py-YI(n)).^2))+Sigma_Beta*randn(M,1);
    Distance(:,n)=sqrt((Px-XI(n)).^2+(Py-YI(n)).^2+(Pz-ZI(n)).^2)+Sigma_Distance*randn(M,1);
end
Rho=Distance(:,2:end)-Distance(:,1)*ones(1,N-1);

%% 使用扩展卡尔曼滤波进行定位
SigmaU=0.005;%驱动噪声
S0=[120,270,320,0.5,0.1,-0.4]';%初始状态向量
P0=0.01*ones(6,6);%预测误差矩阵的初始值
[MX,MY,MZ]=EKF_TDOA_AOA_3D(Alpha,Beta,Rho,XI,YI,ZI,SigmaU,Sigma_Alpha,Sigma_Beta,Sigma_Distance,T,S0,P0);

%% 绘图
figure
plot3(Px,Py,Pz,'b');
xlabel('X轴');
ylabel('Y轴');
zlabel('Z轴');
grid on
axis([0 1000 0 1000 0 300]);
hold on
plot3(MX,MY,MZ,'r');
title('跟踪轨迹');
%
figure
plot(MX-Px);
xlabel('迭代次数');
ylabel('误差(米)');
title('X轴收敛曲线');
grid on
%
figure
plot(MY-Py);
xlabel('迭代次数');
ylabel('误差(米)');
title('Y轴收敛曲线');
grid on
%
figure
plot(MZ-Pz);
xlabel('迭代次数');
ylabel('误差(米)');
title('Z轴收敛曲线');
grid on
%
figure
ERR=sqrt((MX-Px).^2+(MY-Py).^2+(MZ-Pz).^2);
plot(ERR);
xlabel('迭代次数');
ylabel('误差(米)');
title('误差收敛曲线');
grid on

function [X,AllX,Alldxy]=TDOA_AOA_3D(X0,Theta,Phi,Tau,xb,yb,Delta,K)
% GreenSim团队原创作品,转载请注明
% Email:greensim@163.com
% GreenSim团队主页:http://blog.sina.com.cn/greensim
% [color=red]欢迎访问GreenSim——算法仿真团队→[url=http://blog.sina.com.cn/greensim]http://blog.sina.com.cn/greensim[/url][/color]
N=length(Theta);
H=zeros(3*N-1,2*N+2);
Rho=zeros(3*N-1,1);
AllX=zeros(K,2*N+2);
Alldxy=zeros(K,1);
dxy=Inf;
X=X0;
counter=1;
while dxy>Delta
    xm=X(2*N+1);
    ym=X(2*N+2);
    x1=X(1);
    y1=X(2);
    for i=1:N
        xi=X(2*i-1);
        yi=X(2*i);
        if xi==xm
            xi=xm+0.000001;
        end
        if yi==ym
            yi=ym+0.000001;
        end
        if xi==xb
            xi=xb+0.000001;
        end
        if yi==yb
            yi=yb+0.000001;
        end
        H(i,2*i-1)=-(yi-ym)/(xi-xm)^2/(1+(yi-ym)^2/(xi-xm)^2);
        H(i,2*i)=1/(xi-xm)/(1+(yi-ym)^2/(xi-xm)^2);
        H(i,2*N+1)=(yi-ym)/(xi-xm)^2/(1+(yi-ym)^2/(xi-xm)^2);
        H(i,2*N+2)=-1/(xi-xm)/(1+(yi-ym)^2/(xi-xm)^2);
        Rho(i,1)=atan((yi-ym)/(xi-xm));
        H(N+i,2*i-1)=-(yi-yb)/(xi-xb)^2/(1+(yi-yb)^2/(xi-xb)^2);
        H(N+i,2*i)=1/(xi-xb)/(1+(yi-yb)^2/(xi-xb)^2);
        Rho(N+i,1)=atan((yi-yb)/(xi-xb));
        if i>=2
            H(2*N+i-1,1)=-1/2/(x1^2-2*x1*xm+xm^2+y1^2-2*y1*ym+ym^2)^(1/2)*(2*x1-2*xm)-1/2/(x1^2-2*x1*xb+xb^2+y1^2-2*y1*yb+yb^2)^(1/2)*(2*x1-2*xb);
            H(2*N+i-1,2)=-1/2/(x1^2-2*x1*xm+xm^2+y1^2-2*y1*ym+ym^2)^(1/2)*(2*y1-2*ym)-1/2/(x1^2-2*x1*xb+xb^2+y1^2-2*y1*yb+yb^2)^(1/2)*(2*y1-2*yb);
            H(2*N+i-1,2*i-1)=1/2/(xi^2-2*xi*xm+xm^2+yi^2-2*yi*ym+ym^2)^(1/2)*(2*xi-2*xm)+1/2/(xi^2-2*xi*xb+xb^2+yi^2-2*yi*yb+yb^2)^(1/2)*(2*xi-2*xb);
            H(2*N+i-1,2*i)=1/2/(xi^2-2*xi*xm+xm^2+yi^2-2*yi*ym+ym^2)^(1/2)*(2*yi-2*ym)+1/2/(xi^2-2*xi*xb+xb^2+yi^2-2*yi*yb+yb^2)^(1/2)*(2*yi-2*yb);
            H(2*N+i-1,2*N+1)=1/2/(xi^2-2*xi*xm+xm^2+yi^2-2*yi*ym+ym^2)^(1/2)*(-2*xi+2*xm)-1/2/(x1^2-2*x1*xm+xm^2+y1^2-2*y1*ym+ym^2)^(1/2)*(-2*x1+2*xm);
            H(2*N+i-1,2*N+2)=1/2/(xi^2-2*xi*xm+xm^2+yi^2-2*yi*ym+ym^2)^(1/2)*(-2*yi+2*ym)-1/2/(x1^2-2*x1*xm+xm^2+y1^2-2*y1*ym+ym^2)^(1/2)*(-2*y1+2*ym);
            Rho(2*N+i-1,1)=sqrt((xi-xm)^2+(yi-ym)^2)+sqrt((xi-xb)^2+(yi-yb)^2)-sqrt((x1-xm)^2+(y1-ym)^2)-sqrt((x1-xb)^2+(y1-yb)^2);
        end
    end
    Y=[Theta;Phi;Tau]-Rho;
    X=X+DX;
    AllX(counter,:)=X';
    Alldxy(counter,1)=dxy;
    counter=counter+1;
    if counter>K
        return
    end
end
AllX=AllX(1:counter-1,:);
Alldxy=Alldxy(1:counter-1,1);
% GreenSim团队原创作品,转载请注明
% Email:greensim@163.com
% GreenSim团队主页:http://blog.sina.com.cn/greensim

0

阅读 收藏 喜欢 打印举报/Report
  

新浪BLOG意见反馈留言板 欢迎批评指正

新浪简介 | About Sina | 广告服务 | 联系我们 | 招聘信息 | 网站律师 | SINA English | 产品答疑

新浪公司 版权所有