1## Copyright (C) 2001 Paul Kienzle <pkienzle@users.sf.net>
2## Copyright (C) 2004 Pascal Dupuis <Pascal.Dupuis@esat.kuleuven.ac.be>
3##
4## This program is free software: you can redistribute it and/or modify
5## it under the terms of the GNU General Public License as published by
6## the Free Software Foundation, either version 3 of the License, or
7## (at your option) any later version.
8##
9## This program is distributed in the hope that it will be useful,
10## but WITHOUT ANY WARRANTY; without even the implied warranty of
11## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12## GNU General Public License for more details.
13##
14## You should have received a copy of the GNU General Public License
15## along with this program; see the file COPYING. If not, see
16## <https://www.gnu.org/licenses/>.
17
18## -*- texinfo -*-
19## @deftypefn  {Function File} {@var{f} =} sgolay (@var{p}, @var{n})
20## @deftypefnx {Function File} {@var{f} =} sgolay (@var{p}, @var{n}, @var{m})
21## @deftypefnx {Function File} {@var{f} =} sgolay (@var{p}, @var{n}, @var{m}, @var{ts})
22## Computes the filter coefficients for all Savitzsky-Golay smoothing
23## filters of order p for length n (odd). m can be used in order to
24## get directly the mth derivative. In this case, ts is a scaling factor.
25##
26## The early rows of F smooth based on future values and later rows
27## smooth based on past values, with the middle row using half future
28## and half past.  In particular, you can use row i to estimate x(k)
29## based on the i-1 preceding values and the n-i following values of x
30## values as y(k) = F(i,:) * x(k-i+1:k+n-i).
31##
32## Normally, you would apply the first (n-1)/2 rows to the first k
33## points of the vector, the last k rows to the last k points of the
34## vector and middle row to the remainder, but for example if you were
35## running on a realtime system where you wanted to smooth based on the
36## all the data collected up to the current time, with a lag of five
37## samples, you could apply just the filter on row n-5 to your window
38## of length n each time you added a new sample.
39##
40## Reference: Numerical recipes in C. p 650
41##
42## @seealso{sgolayfilt}
43## @end deftypefn
44
45## Based on smooth.m by E. Farhi <manuf@ldv.univ-montp2.fr>
46
47function F = sgolay (p, n, m = 0, ts = 1)
48
49  if (nargin < 2 || nargin > 4)
50    print_usage;
51  elseif rem(n,2) != 1
52    error ("sgolay needs an odd filter length n");
53  elseif p >= n
54    error ("sgolay needs filter length n larger than polynomial order p");
55  else
56    if length(m) > 1, error("weight vector unimplemented"); endif
57
58    ## Construct a set of filters from complete causal to completely
59    ## noncausal, one filter per row.  For the bulk of your data you
60    ## will use the central filter, but towards the ends you will need
61    ## a filter that doesn't go beyond the end points.
62    F = zeros (n, n);
63    k = floor (n/2);
64    for row = 1:k+1
65      ## Construct a matrix of weights Cij = xi ^ j.  The points xi are
66      ## equally spaced on the unit grid, with past points using negative
67      ## values and future points using positive values.
68      C = ( [(1:n)-row]'*ones(1,p+1) ) .^ ( ones(n,1)*[0:p] );
69      ## A = pseudo-inverse (C), so C*A = I; this is constructed from the SVD
70      A = pinv(C);
71      ## Take the row of the matrix corresponding to the derivative
72      ## you want to compute.
73      F(row,:) = A(1+m,:);
74    endfor
75    ## The filters shifted to the right are symmetric with those to the left.
76    F(k+2:n,:) = (-1)^m*F(k:-1:1,n:-1:1);
77
78  endif
79  F =  F * ( prod(1:m) / (ts^m) );
80
81endfunction
82
83%!test
84%! N=2^12;
85%! t=[0:N-1]'/N;
86%! dt=t(2)-t(1);
87%! w = 2*pi*50;
88%! offset = 0.5; # 50 Hz carrier
89%! # exponential modulation and its derivatives
90%! d = 1+exp(-3*(t-offset));
91%! dd = -3*exp(-3*(t-offset));
92%! d2d = 9*exp(-3*(t-offset));
93%! d3d = -27*exp(-3*(t-offset));
94%! # modulated carrier and its derivatives
95%! x = d.*sin(w*t);
96%! dx = dd.*sin(w*t) + w*d.*cos(w*t);
97%! d2x = (d2d-w^2*d).*sin(w*t) + 2*w*dd.*cos(w*t);
98%! d3x = (d3d-3*w^2*dd).*sin(w*t) + (3*w*d2d-w^3*d).*cos(w*t);
99%!
100%! y = sgolayfilt(x,sgolay(8,41,0,dt));
101%! assert(norm(y-x)/norm(x),0,5e-6);
102%!
103%! y = sgolayfilt(x,sgolay(8,41,1,dt));
104%! assert(norm(y-dx)/norm(dx),0,5e-6);
105%!
106%! y = sgolayfilt(x,sgolay(8,41,2,dt));
107%! assert(norm(y-d2x)/norm(d2x),0,1e-5);
108%!
109%! y = sgolayfilt(x,sgolay(8,41,3,dt));
110%! assert(norm(y-d3x)/norm(d3x),0,1e-4);
111