ggk.m

序惯最小二乘法

function y=GGk(k,measure_maxtri)
y=zeros(2,4);
y(1,1)=sin(measure_maxtri(2,k));
y(1,2)=measure_maxtri(1,k)*cos(measure_maxtri(2,k));
y(1,3)=-sin(measure_maxtri(4,k));
y(1,4)=-measure_maxtri(3,k)*cos(measure_maxtri(4,k));
y(2,1)=cos(measure_maxtri(2,k));
y(2,2)=-measure_maxtri(1,k)*sin(measure_maxtri(2,k));
y(2,3)=-cos(measure_maxtri(4,k));
y(2,4)=measure_maxtri(3,k)*sin(measure_maxtri(4,k));