matlab_基于卡尔曼滤波的GNSS卫星导航PVT解算例程。输入参数为至少四颗卫星的XYZ坐标和伪距，输出结果为接收机经纬度值

clc; clear all; close all; %初始条件 Y=load('D:\temp\6_sat.txt'); X0=Y;%%卫星坐标和伪距(读入数据) [m,n]=size(X0); num=fix(n/4);%%卫星个数 t=100;%%仿真时间 TS=1; %%采样时间 len=t/TS; %%步长 dqx=0.5;dqy=0.5;dqz=0.5;dqt=1;dqf=1;%%系统噪声系数（功率谱密度） dqr=5; X=zeros(len,8);%%状态变量矩阵 X(1,:)=[1,1,1,0,0,0,0,0];%%初始状态变量 %X(1,:)=[-200000, 4000000,4000000,0,0,0,0,0];%%初始状态变量 P0=diag([1,1,1,0.5,0.5,0.5,1,1]);%%初始误差值 %观测值 Z=zeros(len,num); for j=1:len%%取伪距的观测值 for ii=1:num aa=ii*4; Z(j,ii)=X0(j,aa);%%第一颗星S01 end % if j>1 % Z(j-1,:)=Z(j,:)-Z(j-1,:); % end end %状态转移矩阵A（状态变量为x,y,z,vx,vy,vz,tu,fu） A=zeros(8,8); A(1:3,1:3)=eye(3); A(1:3,4:6)=TS*eye(3); A(4:6,4:6)=eye(3); A(7:8,7:8)=[1,TS;0,1]; %系统噪声矩阵Q Q=zeros(8,8); q11=[(TS^3/3)*dqx,(TS^3/3)*dqy,(TS^3/3)*dqz]; q12=[(TS^2/2)*dqx,(TS^2/2)*dqy,(TS^2/2)*dqz]; q21=q12; q22=[TS*dqx,TS*dqy,TS*dqz]; Q(1:3,1:3)=diag(q11,0); Q(1:3,4:6)=diag(q12,0); Q(4:6,1:3)=diag(q21,0); Q(4:6,4:6)=diag(q22,0); Q(7:8,7:8)=[dqt*TS+dqf*(TS^3)/3,dqf*(TS^2)/2;dqf*(TS^2)/2,dqf*TS]; %量测噪声矩阵R R=dqr*eye(num); %R=diag([1;1;1;1]); %卡尔曼滤波过程 LC=zeros(len,num);%%量测值 LE=zeros(num,1); X_EST=X;%%卡尔曼滤波值 Pk=P0; for k=2:len xkk_1=YBYC(X(k-1,:),TS, A);%%一步预测(8*1) pk_1=YCWC(Pk,A,Q);%%预测误差方差(8*8) [HKX,Hk]=LCFC(xkk_1,X0(k,:),num);%%量测非线性(HKX为4*1)以及量测方程(4*8) LC(k,:)=HKX'; LE=xkk_1-X(k-1,:)';%%ΔX kg=LBZY(pk_1,Hk,R);%%滤波增益 [xkk,zk]=ZJLB(xkk_1,kg,Z(k,:)',HKX,Hk, LE);%%最佳滤波值xkk(8*1) Pk=LBWCnew(pk_1,kg,Hk,R);%%滤波误差方差 X(k,:)=xkk';%%预测 X_EST(k,:)=xkk'; end m1=n-2;m2=n-1;m3=n; figure(1)%%位置示意图 plot(X_EST(1:t-5,1),'b');ylabel('x'); grid on %figure(1) %hold on;plot(X0(:,m1),':r'); figure(2) plot(X_EST(1:t-5,2),'b');ylabel('y'); grid on %figure(2) %hold on;plot(X0(:,m2),':r'); figure(3) plot(X_EST(1:t-5,3),':b');ylabel('z'); grid on %figure(3) %hold on;plot(X0(:,m3),':r');ylabel('z'); % figure(4)%%速度示意图 % subplot(311) % plot(X_EST(1:180,4),'b');ylabel('vx'); % grid on % % subplot(312) % plot(X_EST(1:180,5),'r');ylabel('vy'); % grid on % % subplot(313) % plot(X_EST(1:180,6),':b');ylabel('vz'); % grid on