程序功能：三基站观测距离EKF与LS算法比较分析MATLAB源码EKF_LS.m

为了评估所提出的基于三基站观测距离EKF融合滤波算法和传统的三基站观测距离LS定位算法的性能，假设系统的过程噪声和观测噪声都是服从均值为零，方差分别为10-8m和10-2m且彼此独立分布的高斯白噪声。已知三个参考基站的位置分别位于为(0，0)、(10，0)和(10，10)处，做匀加速直线运动的目标在初始时刻的状态向量 ，初始状态协方差矩阵 为6×6的单位矩阵。EKF融合滤波算法和LS定位算法的定位性能仿真结果如图1所示，图2展示了EKF融合滤波算法和LS定位算法的定位误差，图3展示了EKF融合滤波算法和LS定位算法的定位误差累积分布函数CDF。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 程序功能：三基站观测距离EKF与LS算法比较分析
clc;clear;close all;
T=1;% 采样时间间隔
N=50/T;% 总的采样次数
F=[10T0（T^2）/20;010T0（T^2）/2;0010T0;00010T;000010;000001];% 状态转移矩阵
delta_q=1e-8;% 如果增大这个参数，目标真实轨迹就是曲线了
Q=delta_q*eye（6）;% 过程噪声方差 
W=sqrt（Q）*randn（6N）;
delta_r=1e-2;
R=delta_r*eye（3）;% 观测噪声方差
V=sqrtm（R）*randn（3N）;
Node_number=3;% 基站个数
Station=[01010;0010];% 3个基站位置初始化
X=zeros（6N）;
Z=zeros（3N）;
X（:1）=[0;0;0.15;0.15;0.002;0.002];% 目标状态位置、速度、加速度初始化
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%基于3个基站观测距离的目标跟踪LS状态估计
for k=1:N-1
    X（:k+1）=F*X（:k）+W（:k）;% 状态方程建模，模拟受到过程噪声的污染
end
for t=1:N
    for i=1:Node_number
        d（i:）=Dist（X（:t）Station（:i））;% 多个观测站对t时刻目标距离测量
    end
    Z（:t）=d+V（:t）;% 观测方程的建模，模拟受到观测噪声的污染
    for i=2:Node_number
        H（i-1:）=[2*（Station（1i）-Station（11））2*（Station（2i）-Station（21））];
        b（i-1:）=[Z（1t）^2-Z（it）^2+Station（1i）^2+Station（2i）^2-Station（11）^2-Station（21）^2];
    end  
    Xls（:t）=inv（H‘*H）*H‘*b;% 最小二乘法得到目标当前时刻的位置
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%基于3个基站观测距离的目标跟踪EKF状态估计
P0=eye（6）;% 协方差矩阵初始化
Xekf（:1）=X（:1）;% 卡尔曼滤波状态初始化
for t=2:N 
    Xn=F*Xekf（:t-1）;% 状态预测
    Zn=[Dist（XnStation（:1））;
        Dist（XnStation（:2））;
        Dist（XnStation（:3））;
       ];% 观测预测
    P1=F*P0*F‘+Q;% 误差协方差预测
    H=[（Xn（1）-Station（11））/Zn（1）（Xn（2）-Station（21））/Zn（1）0000;
       （Xn（1）-Station（12））/Zn（2）（Xn（2）-Station（22））/Zn（2）0000;
       （Xn（1）-Station（13））/Zn（3）（Xn（2）-Station（23））/Zn（3）0000;
       ];%求雅可比矩阵，即为所求一阶近似
    K=P1*H‘/（H*P1*H‘+R）;%卡尔曼滤波增益
    Xekf（:t）=Xn+K*（Z（:t）-Zn）;%状态更新