fdiv.m

文件夹中NPFMain.m为滤波算法主运行程序

%fDiv.m
%本子函数的功能是求取雅可比矩阵
function Result=fDiv(State)
global T;
global Phi;
global G;
global g;
global q;
global Beta;
Result=zeros(2,4);
Result(1,1)=0;
Result(2,1)=0;
Result(1,2)=-0.5*g/State(5)*Rho(State(3))*(2*State(2)^2+State(4)^2)/sqrt(State(2)^2+State(4)^2);
Result(2,2)=-0.5*g/State(5)*Rho(State(3))*State(2)*State(4)/sqrt(State(2)^2+State(4)^2);
Result(1,3)=0.5*g/State(5)*Rho(State(3))*c2(State(3))*State(2)*sqrt(State(2)^2+State(4)^2);
Result(2,3)=0.5*g/State(5)*Rho(State(3))*c2(State(3))*State(4)*sqrt(State(2)^2+State(4)^2);
Result(1,4)=Result(2,2);
Result(2,4)=-0.5*g/State(5)*Rho(State(3))*(2*State(4)^2+State(2)^2)/sqrt(State(2)^2+State(4)^2);