www.gusucode.com > 扩展卡尔曼滤波源码程序 > 扩展卡尔曼滤波源码程序/EKF/EKF/PseudorangeEquation.m
% Compute Val = || Xs - X || + b and its Jacobian. function [Val, Jacob] = PseudorangeEquation(X, SV) % Each row of SV is the coordinate of a satellite. dX = bsxfun(@minus, X([1,3,5])', SV);% X - Xs Val = sum(dX .^2, 2) .^0.5 + X(7); Jacob = zeros(size(SV, 1), size(X, 1)); Jacob(:, [1,3,5]) = bsxfun(@rdivide, dX, Val); Jacob(:, 7) = 1; end