ffun.m

来自「一个关于卫星导航方面的ekf滤波程序」· M 代码 · 共 21 行

function new_state = ffun(state, V)
% EKF-System Function
% Input: Current state, System noise
% Output: New state

[row col]=size(state);
tspan=[1,26];
for k=1:col
    [tt,new]=rk4('threebdyna',[0,12,0.5],state(:,k));%ode45(@DyDt, tspan, state(:,k));
    new_sta(:,:,k)=new(1:24,:);
end
if col==1
    new_state = new_sta(24,:,:)'+ V;
else 
    for k=1:col
        for i=1:6
            new_state(i,k) = new_sta(24,i,k)+V(i,k);
        end
    end
    %new_state = new_state';
end