kalmanxyzt.m

这是很好的一段程序

%扩展卡尔曼滤波　伪距法定位
format long
clear
clc

%假设真实的P坐标与tu值，以作参考
c=3.0e8;              %光速
pu=[1,2,3];           %假定用户接收机的真实坐标
tu=5e-3;              %假定真实的钟差

%4颗卫星真实位置
s1=[5000,4000,6000];
s2=[-1000,5000,3000];
s3=[1000,3000,3000];
s4=[-3000,4500,3000];

%伪距p(没有噪声影响)
t=s1-pu; p(1)=sqrt(sum(t.*t))+c*tu;
t=s2-pu; p(2)=sqrt(sum(t.*t))+c*tu;
t=s3-pu; p(3)=sqrt(sum(t.*t))+c*tu;
t=s4-pu; p(4)=sqrt(sum(t.*t))+c*tu;

MAX=100;%数据量个数
RD=randn(MAX,4)/100;%下面程序中将用到，产生随机噪声，使伪距显得更“实际”


%卡尔曼滤波器初始值
T(1).pu=[10,20,30]; T(1).tu=6e-3;
T(1).X = ( [T(1).pu,T(1).tu] )';  %用户位置初始值、接收机钟差初始值    T(k).X_ ( 4 X 1 矩阵)
T(1).P = 0;

R=0.01*eye(4);     % 4 X 4 矩阵
Q=1e-5*eye(4);     % 4 X 4 矩阵


for k=2:1:MAX
    %带噪声的伪距
    T(k).Z  = ( p+RD(k,:) )';  %观测量  4 X 1 矩阵
    T(k).X_ = T(k-1).X ;      %状态量
    T(k).pu = [T(k).X_(1),T(k).X_(2),T(k).X_(3)];
    T(k).tu = T(k).X_(4);
    
    %近似伪距p_
    t=s1-T(k).pu;     r_(1)=sqrt(sum(t.*t));    p_(1)=r_(1)+c*T(k).tu;   
    t=s2-T(k).pu;     r_(2)=sqrt(sum(t.*t));    p_(2)=r_(2)+c*T(k).tu;
    t=s3-T(k).pu;     r_(3)=sqrt(sum(t.*t));    p_(3)=r_(3)+c*T(k).tu;
    t=s4-T(k).pu;     r_(4)=sqrt(sum(t.*t));    p_(4)=r_(4)+c*T(k).tu;
    T(k).Z_ = p_' ;
    
    %构造H矩阵
    H=c*ones(4,4);
    for i=1:3
        H(1,i)=-(s1(i)-T(k).pu(i))/r_(1);     %a1(i)
        H(2,i)=-(s2(i)-T(k).pu(i))/r_(2);     %a2(i)
        H(3,i)=-(s3(i)-T(k).pu(i))/r_(3);     %a3(i)
        H(4,i)=-(s4(i)-T(k).pu(i))/r_(4);     %a4(i)
    end 
    T(k).H=H;
    
    T(k).P_ = T(k-1).P + Q ; 
    T(k).Kg = T(k).P_ * T(k).H' / ( T(k).H*T(k).P_*T(k).H'+ R ) ;     %卡尔曼增益
    T(k).X  = T(k).X_ + T(k).Kg * ( T(k).Z - T(k).Z_ );               %观测变量的更新
    T(k).P  = (1 - T(k).Kg * T(k).H) * T(k).P_ ;     
    
end  %end of for

%作图
figure(1);
clf reset;
for k=1:MAX;
    x(k)=T(k).pu(1);
    y(k)=T(k).pu(2);
    z(k)=T(k).pu(3);
end
k=1:MAX;  
plot3(x(k),y(k),z(k),'r.');
axis on;
grid on;