%DER_CHECK Check derivatives using finite differences
%
% Syntax:
% [D0,D1] = DER_CHECK(F,DF,INDEX,[P1,P2,P3,...])
%
% In:
% F - Name of actual function or inline function
% in form F(P1,P2,...)
% DF - Derivative value as matrix, name of derivative
% function or inline function in form DF(P1,P2,...).
% INDEX - Index of parameter of interest. DF should
% Calculate the derivative with recpect to parameter
% Pn, where n is the index.
%
% Out:
% D0 - Actual derivative
% D1 - Estimated derivative
%
% Description:
% Evaluates function derivative analytically and
% using finite differences. If no output arguments
% are given, issues a warning if these two values
% differ too much.
%
% Function is intended to checking that derivatives
% of transition and measurement equations of EKF are
% bug free.
%
% See also:
% EKF_PREDICT1, EKF_UPDATE1, EKF_PREDICT2, EKF_UPDATE2
% History:
% 12.03.2003 SS Support for function handles
% 27.11.2002 SS The first official version.
%
% Copyright (C) 2002 Simo Särkkä
%
% $Id: der_check.m 111 2007-09-04 12:09:23Z ssarkka $
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.
function [D0,D1] = der_check(f,df,index,varargin)
%
% Calculate function value and derivative
%
if isstr(f) | strcmp(class(f),'function_handle')
y0 = feval(f,varargin{:});
else
y0 = f(varargin{:});
end
if isnumeric(df)
D0 = df;
elseif isstr(df) | strcmp(class(df),'function_handle')
D0 = feval(df,varargin{:});
else
D0 = df(varargin{:});
end
%
% Calculate numerical derivative
%
h = 0.0000001;
D1 = [];
X = varargin{index};
for r=1:size(X,1)
for c=1:size(X,2)
H = zeros(size(X,1),size(X,2));
H(r,c) = h;
varargin{index} = X+H;
if isstr(f) | strcmp(class(f),'function_handle')
y1 = feval(f,varargin{:});
else
y1 = f(varargin{:});
end
d1 = (y1-y0)/h;
if size(d1,1)>1
D1 = [D1 d1];
else
D1(r,c) = d1;
end
end
end
if nargout == 0
d = abs(D1(:)-D0(:));
if max(d) > 0.001
warning('Derivatives differ too much');
D0
D1
else
fprintf('Derivative check passed.\n');
end
end