function sol_kf(filename) global glc; %read solution file solution=readGINavsol(filename); n=size(solution,1); %initialize 对状态进行初始化 x = zeros(6,1);%state vector 状态向量 xP = zeros(6,6);%covariance of x 协方差 %过程噪声密度 sqrt_accPSD_hor=15;%m/s^2/sqrt(Hz) sqrt_accPSD_up=0.1;%m/s^2/sqrt(Hz) accPSD_enu =zeros(3,3); accPSD_enu(1,1)=(sqrt_accPSD_hor)^2; accPSD_enu(2,2)=(sqrt_accPSD_hor)^2; accPSD_enu(3,3)=sqrt_accPSD_up^2; bInitialized=0; time_x = solution(1).time; %all epochs for i=1:n %load data of new epoch sol = solution(i); %转换矩阵 Cne=xyz2enu(sol.pos); %转换至地球坐标系下 accPSD=Cne'*accPSD_enu*Cne; vel=sol.vel'; pos=sol.pos'; %临时变量进行存储 posP=[sol.posP(1) sol.posP(4) sol.posP(6); sol.posP(4) sol.posP(2) sol.posP(5); sol.posP(6) sol.posP(5) sol.posP(3)]; velP=[sol.velP(1) sol.velP(4) sol.velP(6); sol.velP(4) sol.velP(2) sol.velP(5); sol.velP(6) sol.velP(5) sol.velP(3)]; dt = timediff(sol.time,time_x); %如果时间差很大，说明时间中断很久，则需要重新进行初始化 if dt>15 bInitialized=0; end %initialize if ~bInitialized x = [pos; vel]; xP=zeros(6,6); xP(1:3,1:3)= eye(3)*1e16; xP(4:6,4:6)= eye(3)*100; if sol.posP(1)>0 xP(1:3,1:3)= posP; end if sol.velP(1)>0 xP(4:6,4:6)= velP; end time_x = sol.time; bInitialized=1; continue; end %%%%%%%%%%%%%%%to do%%%%%%%%%%%%%%%%%%%%%%%%% %step 1: prediction %Phi %Q 补全该代码

%step 1: prediction
%Phi
dt = timediff(sol.time,time_x); %计算两个历元的时间差
F=zeros(6,6);
F(1:3,4:6)=eye(3)*dt;
Phi = [eye(3) eye(3)*dt; zeros(3,3) eye(3)];
%Q
Q=zeros(6,6);
Q(1:3,1:3)=eye(3)*0.25*dt^4*accPSD(1,1)+eye(3)*0.5*dt^3*accPSD(1,1); %水平方向
Q(1:3,4:6)=eye(3)*0.5*dt^2*accPSD(1,1); %水平方向
Q(4:6,1:3)=eye(3)*0.5*dt^2*accPSD(1,1); %水平方向
Q(4:6,4:6)=eye(3)*dt^2*accPSD(3,3); %垂直方向
%预测状态和协方差
x = Phi * x;
xP = Phi * xP * Phi' + Q;

%step 2: measurement update
if sol.posP(1)>0 %如果有位置信息
H = [eye(3) zeros(3,3)]; %测量矩阵
R=zeros(3,3);
R(1:2,1:2)=eye(2)*sol.posP(1)^2; %水平误差
R(3,3)=sol.posP(7)^2; %垂直误差
%计算卡尔曼增益
K = xP * H' * inv(H * xP * H' + R);
%测量更新状态和协方差
z = sol.pos';
dz = z - H * x;
dz(3) = rem(dz(3)+pi,2*pi)-pi; %处理航向角差异
x = x + K * dz;
xP = (eye(6) - K * H) * xP;
end

%更新历元时间
time_x = sol.time;