diff --git a/README.md b/README.md index a8ed6aa..76c1ae0 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -EKF/UKF Toolbox for Matlab +EKF/UKF Toolbox for Matlab and GNU Octave == [Simo Särkkä](http://users.aalto.fi/~ssarkka/), Jouni Hartikainen, and [Arno Solin](http://arno.solin.fi) @@ -14,7 +14,26 @@ Most of the code has been written by Prof. Simo Särkkä. Later Dr. Jouni Hartik Download and Installation Guide -- -The software consists of Matlab m-files. Clone or download the latest version and make sure the toolbox directory is included in your Matlab path by `addpath` *path to ekfukf*. +Matlab +--- + +The software consists of Matlab m-files. +Clone or download the latest version and make sure the toolbox directory is included in +your Matlab path by `addpath` *path to ekfukf*. + +GNU Octave +--- + +1. Run the following command + + make dist + + This will create a file `ekfukf-.tar.gz` + +2. Install the package. + Start octave and run + + pkg install ekfukf-.tar.gz Documentation @@ -61,4 +80,4 @@ Demonstration programs for multiple model systems: License -- -This software is distributed under the GNU General Public License (version 2 or later); please refer to the file `LICENSE.txt`, included with the software, for details. \ No newline at end of file +This software is distributed under the GNU General Public License (version 2 or later); please refer to the file `LICENSE.txt`, included with the software, for details. diff --git a/demos/eimm_demo/botm_demo.m b/demos/eimm_demo/botm_demo.m index 16648d3..d3c262f 100644 --- a/demos/eimm_demo/botm_demo.m +++ b/demos/eimm_demo/botm_demo.m @@ -138,7 +138,7 @@ % Generate object state values for i = 2:n st = mstate(i); - if isstr(a_func{st}) | strcmp(class(a_func{st}),'function_handle') + if ischar(a_func{st}) | strcmp(class(a_func{st}),'function_handle') X_r(ind{st},i) = feval(a_func{st},X_r(ind{st},i-1),a_param{st}); else X_r(ind{st},i) = A{st}*X_r(ind{st},i-1); diff --git a/demos/eimm_demo/ct_demo.m b/demos/eimm_demo/ct_demo.m index 1d1b3dc..a29aa61 100644 --- a/demos/eimm_demo/ct_demo.m +++ b/demos/eimm_demo/ct_demo.m @@ -149,7 +149,7 @@ % Generate object state values for i = 2:n st = mstate(i); - if isstr(a_func{st}) | strcmp(class(a_func{st}),'function_handle') + if ischar(a_func{st}) | strcmp(class(a_func{st}),'function_handle') X_r(ind{st},i) = feval(a_func{st},X_r(ind{st},i-1),a_param{st}); else X_r(ind{st},i) = A{st}*X_r(ind{st},i-1); diff --git a/demos/kf_cwpa_demo/kf_cwpa_demo.m b/demos/kf_cwpa_demo/kf_cwpa_demo.m index b55e91f..4565f7c 100644 --- a/demos/kf_cwpa_demo/kf_cwpa_demo.m +++ b/demos/kf_cwpa_demo/kf_cwpa_demo.m @@ -2,11 +2,15 @@ % % Copyright (C) 2007 Jouni Hartikainen % -% This software is distributed under the GNU General Public -% Licence (version 2 or later); please refer to the file +% 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 kf_cwpa_demo +function kf_cwpa_demo (varargin) +doprint = false; % whether we print figures of results +if nargin >= 1 + doprint = true; +end % Transition matrix for the continous-time system. F = [0 0 1 0 0 0; @@ -15,6 +19,7 @@ 0 0 0 0 0 1; 0 0 0 0 0 0; 0 0 0 0 0 0]; +nF = size (F, 1); % Noise effect matrix for the continous-time system. L = [0 0; @@ -28,247 +33,257 @@ dt = 0.5; % Process noise variance -q = 0.2; -Qc = diag([q q]); +q = 0.2; +Qc = diag ([q q]); % Discretization of the continous-time system. [A,Q] = lti_disc(F,L,Qc,dt); % Measurement model. -H = [1 0 0 0 0 0; - 0 1 0 0 0 0]; +H = [1 0 0 0 0 0; + 0 1 0 0 0 0]; +nH = size (H, 1); % Variance in the measurements. r1 = 10; r2 = 5; -R = diag([r1 r1]); +R = diag ([r1 r1]); % Generate the data. -n = 50; -Y = zeros(size(H,1),n); -X_r = zeros(size(F,1),n); +n = 50; +X_r = zeros (nF, n); X_r(:,1) = [0 0 0 0 0 0]'; +tmp = zeros (nF, 1); for i = 2:n - X_r(:,i) = A*X_r(:,i-1) + gauss_rnd(zeros(size(F,1),1), Q); + X_r(:,i) = A * X_r(:,i-1) + gauss_rnd (tmp, Q); end % Generate the measurements. +Y = zeros (nH, n); +tmp = zeros (nH, 1); for i = 1:n - Y(:,i) = H*X_r(:,i) + gauss_rnd(zeros(size(Y,1),1), R); + Y(:,i) = H * X_r(:,i) + gauss_rnd (tmp, R); end clf; clc; -disp('This is a demonstration program for the classical Kalman filter.'); -disp(' '); -disp(['KF is used here to estimate the position of a moving object, whos ',... - 'dynamics follow the CWPA-model described in the documentation ',... - 'provided with the toolbox.']); -disp(' '); -disp(['We get noisy measurements from the objects position and velocity ',... - 'on discrete time steps. The real position of the object and the ',... - 'measurements made of them are displayed now. The blue line is the ',... - 'real path of the object and the green dots represents the ',... - 'measurements. The red circle represents the starting point ',... - 'of the object.']); - -disp(' '); -fprintf('Filtering with KF...'); - -plot(X_r(1,:),X_r(2,:),Y(1,:),Y(2,:),'.',X_r(1,1),... +msg_txt = ['This is a demonstration program for the classical Kalman filter.\n', ... + 'KF is used here to estimate the position of a moving object, whos\n',... + 'dynamics follow the CWPA-model described in the documentation\n',... + 'provided with the toolbox.\n', ... + 'We get noisy measurements from the objects position and velocity\n',... + 'on discrete time steps. The real position of the object and the\n',... + 'measurements made of them are displayed now. The blue line is the\n',... + 'real path of the object and the green dots represents the\n',... + 'measurements. The red circle represents the starting point\n',... + 'of the object.\n\n']; +fprintf (msg_txt); +fprintf('\nFiltering with KF... '); + +plot (X_r(1,:),X_r(2,:),Y(1,:),Y(2,:),'.',X_r(1,1),... X_r(2,1),'ro','MarkerSize',12); -legend('Real trajectory', 'Measurements'); -title('Position'); +legend ('Real trajectory', 'Measurements'); +title ('Position'); +axis tight -% Uncomment if you want to save an image -% print -dpsc demo1_f1.ps +if doprint% save an image + print -dpsc demo1_f1.ps +end % Initial guesses for the state mean and covariance. m = [0 0 0 0 0 0]'; -P = diag([0.1 0.1 0.1 0.1 0.5 0.5]); +P = diag ([0.1 0.1 0.1 0.1 0.5 0.5]); %% Space for the estimates. -MM = zeros(size(m,1), size(Y,2)); -PP = zeros(size(m,1), size(m,1), size(Y,2)); +MM = zeros (size(m,1), size(Y,2)); +PP = zeros (size(m,1), size(m,1), size(Y,2)); % Filtering steps. for i = 1:size(Y,2) - [m,P] = kf_predict(m,P,A,Q); - [m,P] = kf_update(m,P,Y(:,i),H,R); - MM(:,i) = m; + [m,P] = kf_predict (m, P, A, Q); + [m,P] = kf_update (m, P, Y(:,i), H, R); + MM(:,i) = m; PP(:,:,i) = P; end % Smoothing step. -[SM,SP] = rts_smooth(MM,PP,A,Q); -[SM2,SP2] = tf_smooth(MM,PP,Y,A,Q,H,R,1); +[SM,SP] = rts_smooth (MM,PP,A,Q); +[SM2,SP2] = tf_smooth (MM,PP,Y,A,Q,H,R,1); -fprintf('ready.\n'); -disp(' '); -disp(''); -pause +fprintf('ready.\n\n'); +input (''); -subplot(1,2,1); -plot(X_r(1,:), X_r(2,:),'--', MM(1,:), MM(2,:),X_r(1,1),X_r(2,1),... +subplot (1,2,1); +plot (X_r(1,:), X_r(2,:),'--', MM(1,:), MM(2,:),X_r(1,1),X_r(2,1),... 'o','MarkerSize',12) -legend('Real trajectory', 'Filtered'); -title('Position estimation with Kalman filter.'); -xlabel('x'); -ylabel('y'); +legend ('Real trajectory', 'Filtered'); +title ('Position estimation with Kalman filter.'); +xlabel ('x'); +ylabel ('y'); -subplot(1,2,2); -plot(X_r(3,:), X_r(4,:),'--', MM(3,:), MM(4,:),X_r(3,1),... +subplot (1,2,2); +plot (X_r(3,:), X_r(4,:),'--', MM(3,:), MM(4,:),X_r(3,1),... X_r(4,1),'ro','MarkerSize',12); -legend('Real velocity', 'Filtered'); -title('Velocity estimation with Kalman filter.'); -xlabel('x^.'); -ylabel('y^.'); - -% Uncomment if you want to save an image -% print -dpsc demo1_f2.ps - +legend ('Real velocity', 'Filtered'); +title ('Velocity estimation with Kalman filter.'); +xlabel ('x^.'); +ylabel ('y^.'); -clc; -disp(['The filtering results are displayed now. In the left panel the ',... - 'estimated path is shown, and, for comparison, in the right panel ',... - 'the estimated velocity is shown.']); -disp(' ') -disp(''); +if doprint % save an image + print -dpsc demo1_f2.ps +end -pause -subplot(1,2,1); -plot(X_r(1,:), X_r(2,:),'--', SM(1,:), SM(2,:),X_r(1,1),... +clc; +msg_txt = ['The filtering results are displayed now. In the left panel the\n',... + 'estimated path is shown, and, for comparison, in the right panel\n',... + 'the estimated velocity is shown.\n\n']; +fprintf (msg_txt); +input(''); + +subplot (1,2,1); +plot (X_r(1,:), X_r(2,:),'--', SM(1,:), SM(2,:),X_r(1,1),... X_r(2,1),'ro','MarkerSize',12); -legend('Real trajectory', 'Smoothed'); -title('Position estimation with RTS smoother.'); -xlabel('x'); -ylabel('y'); +legend ('Real trajectory', 'Smoothed'); +title ('Position estimation with RTS smoother.'); +xlabel ('x'); +ylabel ('y'); -subplot(1,2,2); -plot(X_r(3,:), X_r(4,:),'--', SM(3,:), SM(4,:),X_r(3,1),... +subplot (1,2,2); +plot (X_r(3,:), X_r(4,:),'--', SM(3,:), SM(4,:),X_r(3,1),... X_r(4,1),'ro','MarkerSize',12); -legend('Real velocity', 'Smoothed'); -title('Velocity estimation with RTS smoother.'); -xlabel('x^.'); -ylabel('y^.'); - -% Uncomment if you want to save an image -% print -dpsc demo1_f3.ps - -clc; -disp(['The smoothing results are displayed now. In the left panel the ',... - 'estimated path is shown, and, for comparison, in the right panel ',... - 'the estimated velocity is shown.']); -disp(' ') -disp(''); - -pause -subplot(1,2,1); -plot(X_r(1,:), X_r(2,:),'--', SM2(1,:), SM2(2,:),X_r(1,1),... - X_r(2,1),'ro','MarkerSize',12); -legend('Real trajectory', 'Smoothed'); -title('Position estimation with TF smoother.'); -xlabel('x'); -ylabel('y'); +legend ('Real velocity', 'Smoothed'); +title ('Velocity estimation with RTS smoother.'); +xlabel ('x^.'); +ylabel ('y^.'); +axis tight + +if doprint % save an image + print -dpsc demo1_f3.ps +end -subplot(1,2,2); -plot(X_r(3,:), X_r(4,:),'--', SM2(3,:), SM2(4,:),X_r(3,1),... +clc; +msg_txt = ['The smoothing results are displayed now. In the left panel the\n',... + 'estimated path is shown, and, for comparison, in the right panel\n',... + 'the estimated velocity is shown.\n\n']; +fprintf (msg_txt) +input (''); + +subplot (1,2,1); +plot (X_r(1,:), X_r(2,:),'--', SM2(1,:), SM2(2,:),X_r(1,1),... + X_r(2,1),'ro','MarkerSize',12); +legend ('Real trajectory', 'Smoothed'); +title ('Position estimation with TF smoother.'); +xlabel ('x'); +ylabel ('y'); +axis tight + +subplot (1,2,2); +plot (X_r(3,:), X_r(4,:),'--', SM2(3,:), SM2(4,:),X_r(3,1),... X_r(4,1),'ro','MarkerSize',12); -legend('Real velocity', 'Smoothed'); -title('Velocity estimation with TF smoother.'); -xlabel('x^.'); -ylabel('y^.'); +legend ('Real velocity', 'Smoothed'); +title ('Velocity estimation with TF smoother.'); +xlabel ('x^.'); +ylabel ('y^.'); +axis tight -% Track and animate the filtering result. +% Track and animate the filtering result. clc -disp(['Filtering result is now displayed sequentially on ',... - 'every time step.']); -disp(' '); -disp(['The observations are displayed as green dots and ',... - 'the real signal as solid blue line. The filtering ',... - 'result of previous steps is plotted as dashed black ',... - 'line. The covariance in each step is displayed as a ',... - 'green ellipse around the mean (black circle) on each ',... - 'step. The predicted position on next step is displayed ',... - 'as a red circle']); -disp(' '); -disp(''); +msg_txt = ['Filtering result is now displayed sequentially on every time step.\n', ... + 'The observations are displayed as green dots and\n',... + 'the real signal as solid blue line. The filtering\n',... + 'result of previous steps is plotted as dashed black\n',... + 'line. The covariance in each step is displayed as a\n',... + 'green ellipse around the mean (black circle) on each\n',... + 'step. The predicted position on next step is displayed\n',... + 'as a red circle.\n\n']; +fprintf (msg_txt); +input (''); clf EST = []; -for k=1:size(Y,2) - M = MM(:,k); - P = PP(:,:,k); +tt = (0:0.01:1) *2 * pi; +nt = length (tt); +CC = [cos(tt);sin(tt)]; +for k = 1:size(Y,2) + M = MM(:,k); + P = PP(:,:,k); EST = [EST M]; M_pred = kf_predict(M,P,A,Q); - + % Confidence ellipse - tt = (0:0.01:1)*2*pi; - cc = repmat(M(1:2),1,length(tt)) + ... - 2*chol(P(1:2,1:2))'*[cos(tt);sin(tt)]; + cc = repmat (M(1:2),1, nt) + 2 * chol (P(1:2,1:2))' * CC; % Animate - plot(X_r(1,:),X_r(2,:),'-',... - Y(1,:), Y(2,:), '.',... - M(1),M(2),'ko',... - M_pred(1),M_pred(2),'ro',... - EST(1,:),EST(2,:),'k--',... - cc(1,:),cc(2,:),'g-'); - drawnow; + if k == 1 + hndl = plot(X_r(1,:),X_r(2,:),'-',... + Y(1,:), Y(2,:), '.',... + M(1),M(2),'ko',... + M_pred(1),M_pred(2),'ro',... + EST(1,:),EST(2,:),'k--',... + cc(1,:),cc(2,:),'g-'); + drawnow; + axis tight + else + set (hndl(3), 'xdata', M(1), 'ydata', M(2)); + set (hndl(4), 'xdata', M_pred(1), 'ydata', M_pred(2)); + set (hndl(5), 'xdata', EST(1,:), 'ydata', EST(2,:)); + set (hndl(6), 'xdata', cc(1,:), 'ydata', cc(2,:)); + end pause; end % Smoothing result. clf; clc; -disp(['Smoothing (using RTS smoother) result is now displayed ',... - 'sequentially on every time step in reverse order.']); -disp(' '); -disp(['The observations are displayed as green dots and the ',... - 'real signal as solid blue line. The smoothing result ',... - 'of previous steps is plotted as dashed black line.',... - 'The covariance in each step is displayed as a green',... - 'ellipse around the mean (black circle) on each step.']); -disp(' '); -disp(''); +msg_txt = ['Smoothing (using RTS smoother) result is now displayed\n',... + 'sequentially on every time step in reverse order.\n', ... + 'The observations are displayed as green dots and the\n',... + 'real signal as solid blue line. The smoothing result\n',... + 'of previous steps is plotted as dashed black line.\n',... + 'The covariance in each step is displayed as a green\n',... + 'ellipse around the mean (black circle) on each step.\n\n'] +fprintf (msg_txt) +input (''); EST = []; -for k=size(Y,2):-1:1 - M = SM(:,k); - P = SP(:,:,k); +for k = size(Y,2):-1:1 + M = SM(:,k); + P = SP(:,:,k); EST = [EST M]; % Confidence ellipse - tt = (0:0.01:1)*2*pi; - cc = repmat(M(1:2),1,length(tt)) + ... - 2*chol(P(1:2,1:2))'*[cos(tt);sin(tt)]; + cc = repmat (M(1:2), 1, nt) + 2 * chol (P(1:2,1:2))' * CC; % Animate - len = 1.5; - plot(X_r(1,:),X_r(2,:),'-',... - Y(1,:),Y(2,:),'.',... - M(1),M(2),'ko',... - EST(1,:),EST(2,:),'k--',... - cc(1,:),cc(2,:),'g-'); - drawnow; + if k == size(Y,2) + hndl = plot(X_r(1,:),X_r(2,:),'-',... + Y(1,:), Y(2,:), '.',... + M(1),M(2),'ko',... + EST(1,:),EST(2,:),'k--',... + cc(1,:),cc(2,:),'g-'); + drawnow; + axis tight + else + set (hndl(3), 'xdata', M(1), 'ydata', M(2)); + set (hndl(4), 'xdata', EST(1,:), 'ydata', EST(2,:)); + set (hndl(5), 'xdata', cc(1,:), 'ydata', cc(2,:)); + end pause; end % MSEs of position estimates -MSE_KF1 = mean((X_r(1,:)-MM(1,:)).^2); -MSE_KF2 = mean((X_r(2,:)-MM(2,:)).^2); -MSE_KF = 1/2*(MSE_KF1 + MSE_KF2); +MSE_KF1 = mean ((X_r(1,:) - MM(1,:)).^2); +MSE_KF2 = mean ((X_r(2,:) - MM(2,:)).^2); +MSE_KF = 1/2 * (MSE_KF1 + MSE_KF2); -MSE_RTS1 = mean((X_r(1,:)-SM(1,:)).^2); -MSE_RTS2 = mean((X_r(2,:)-SM(2,:)).^2); -MSE_RTS = 1/2*(MSE_RTS1 + MSE_RTS2); +MSE_RTS1 = mean ((X_r(1,:) - SM(1,:)).^2); +MSE_RTS2 = mean ((X_r(2,:) - SM(2,:)).^2); +MSE_RTS = 1/2 * (MSE_RTS1 + MSE_RTS2); -MSE_TF1 = mean((X_r(1,:)-SM2(1,:)).^2); -MSE_TF2 = mean((X_r(2,:)-SM2(2,:)).^2); -MSE_TF = 1/2*(MSE_TF1 + MSE_TF2); +MSE_TF1 = mean ((X_r(1,:) - SM2(1,:)).^2); +MSE_TF2 = mean ((X_r(2,:) - SM2(2,:)).^2); +MSE_TF = 1/2 * (MSE_TF1 + MSE_TF2); clc; fprintf('Mean square errors of position estimates:\n'); fprintf('KF-RMSE = %.4f\n',MSE_KF); fprintf('RTS-RMSE = %.4f\n',MSE_RTS); fprintf('TF-RMSE = %.4f\n\n',MSE_TF); - - diff --git a/der_check.m b/der_check.m index 1577578..d4f2539 100644 --- a/der_check.m +++ b/der_check.m @@ -45,14 +45,14 @@ % % Calculate function value and derivative % - if isstr(f) | strcmp(class(f),'function_handle') + if ischar(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') + elseif ischar(df) | strcmp(class(df),'function_handle') D0 = feval(df,varargin{:}); else D0 = df(varargin{:}); @@ -69,7 +69,7 @@ H = zeros(size(X,1),size(X,2)); H(r,c) = h; varargin{index} = X+H; - if isstr(f) | strcmp(class(f),'function_handle') + if ischar(f) | strcmp(class(f),'function_handle') y1 = feval(f,varargin{:}); else y1 = f(varargin{:}); diff --git a/eimm_smooth.m b/eimm_smooth.m index ef6958d..7dcd258 100644 --- a/eimm_smooth.m +++ b/eimm_smooth.m @@ -139,7 +139,7 @@ % Retrieve the transition matrix or the Jacobian of the dynamic model if isnumeric(A{i2}) A2 = A{i2}; - elseif isstr(A{i2}) | strcmp(class(A{i2}),'function_handle') + elseif ischar(A{i2}) | strcmp(class(A{i2}),'function_handle') A2 = feval(A{i2},x_bki{i2}(ind{i2}),a_param{i2}); else A2 = A{i2}(x_bki{i2}(ind{i2}),a_param{i2}); diff --git a/ekf_predict1.m b/ekf_predict1.m index 4491d13..6ce0351 100644 --- a/ekf_predict1.m +++ b/ekf_predict1.m @@ -73,7 +73,7 @@ if isnumeric(A) % nop - elseif isstr(A) | strcmp(class(A),'function_handle') + elseif ischar(A) | strcmp(class(A),'function_handle') A = feval(A,M,param); else A = A(M,param); @@ -86,7 +86,7 @@ M = A*M; elseif isnumeric(a) M = a; - elseif isstr(a) | strcmp(class(a),'function_handle') + elseif ischar(a) | strcmp(class(a),'function_handle') M = feval(a,M,param); else M = a(M,param); @@ -96,7 +96,7 @@ if isnumeric(W) % nop - elseif isstr(W) | strcmp(class(W),'function_handle') + elseif ischar(W) | strcmp(class(W),'function_handle') W = feval(W,M,param); else W = W(M,param); diff --git a/ekf_predict2.m b/ekf_predict2.m index 74ed3ff..3406871 100644 --- a/ekf_predict2.m +++ b/ekf_predict2.m @@ -86,7 +86,7 @@ if isnumeric(A) % nop - elseif isstr(A) | strcmp(class(A),'function_handle') + elseif ischar(A) | strcmp(class(A),'function_handle') A = feval(A,M,param); else A = A(M,param); @@ -94,7 +94,7 @@ if isnumeric(F) % nop - elseif isstr(F) | strcmp(class(F),'function_handle') + elseif ischar(F) | strcmp(class(F),'function_handle') F = feval(F,M,param); else F = F(M,param); @@ -105,7 +105,7 @@ M = A*M; elseif isnumeric(a) M = a; - elseif isstr(a) | strcmp(class(a),'function_handle') + elseif ischar(a) | strcmp(class(a),'function_handle') M = feval(a,M,param); else M = a(M,param); @@ -119,7 +119,7 @@ if isnumeric(W) % nop - elseif isstr(W) | strcmp(class(W),'function_handle') + elseif ischar(W) | strcmp(class(W),'function_handle') W = feval(W,M,param); else W = W(M,param); diff --git a/ekf_update1.m b/ekf_update1.m index 9ca574f..a8566a0 100644 --- a/ekf_update1.m +++ b/ekf_update1.m @@ -75,7 +75,7 @@ % if isnumeric(H) % nop - elseif isstr(H) | strcmp(class(H),'function_handle') + elseif ischar(H) | strcmp(class(H),'function_handle') H = feval(H,M,param); else H = H(M,param); @@ -85,7 +85,7 @@ MU = H*M; elseif isnumeric(h) MU = h; - elseif isstr(h) | strcmp(class(h),'function_handle') + elseif ischar(h) | strcmp(class(h),'function_handle') MU = feval(h,M,param); else MU = h(M,param); @@ -93,7 +93,7 @@ if isnumeric(V) % nop - elseif isstr(V) | strcmp(class(V),'function_handle') + elseif ischar(V) | strcmp(class(V),'function_handle') V = feval(V,M,param); else V = V(M,param); diff --git a/ekf_update2.m b/ekf_update2.m index 92acb12..7eee1fb 100644 --- a/ekf_update2.m +++ b/ekf_update2.m @@ -79,7 +79,7 @@ % if isnumeric(H) % nop - elseif isstr(H) | strcmp(class(H),'function_handle') + elseif ischar(H) | strcmp(class(H),'function_handle') H = feval(H,M,param); else H = H(M,param); @@ -87,7 +87,7 @@ if isnumeric(H_xx) % nop - elseif isstr(H_xx) | strcmp(class(H_xx),'function_handle') + elseif ischar(H_xx) | strcmp(class(H_xx),'function_handle') H_xx = feval(H_xx,M,param); else H_xx = H_xx(M,param); @@ -97,7 +97,7 @@ MU = H*M; elseif isnumeric(h) MU = h; - elseif isstr(h) | strcmp(class(h),'function_handle') + elseif ischar(h) | strcmp(class(h),'function_handle') MU = feval(h,M,param); else MU = h(M,param); @@ -105,7 +105,7 @@ if isnumeric(V) % nop - elseif isstr(V) | strcmp(class(V),'function_handle') + elseif ischar(V) | strcmp(class(V),'function_handle') V = feval(V,M,param); else V = V(M,param); diff --git a/erts_smooth1.m b/erts_smooth1.m index 3551686..93118b6 100644 --- a/erts_smooth1.m +++ b/erts_smooth1.m @@ -122,7 +122,7 @@ m_pred = A*M(:,k); elseif isnumeric(a) m_pred = a; - elseif isstr(a) | strcmp(class(a),'function_handle') + elseif ischar(a) | strcmp(class(a),'function_handle') m_pred = feval(a,M(:,k),params); else m_pred = a(M(:,k),params); @@ -130,7 +130,7 @@ if isnumeric(A) F = A; - elseif isstr(A) | strcmp(class(A),'function_handle') + elseif ischar(A) | strcmp(class(A),'function_handle') F = feval(A,M(:,k),params); else F = A(M(:,k),params); @@ -138,7 +138,7 @@ if isnumeric(W) B = W; - elseif isstr(W) | strcmp(class(W),'function_handle') + elseif ischar(W) | strcmp(class(W),'function_handle') B = feval(W,M(:,k),params); else B = W(M(:,k),params); diff --git a/etf_smooth1.m b/etf_smooth1.m index 6994efa..ca88ddf 100644 --- a/etf_smooth1.m +++ b/etf_smooth1.m @@ -153,7 +153,7 @@ IA = []; elseif isnumeric(A) IA = inv(A); - elseif isstr(A) | strcmp(class(A),'function_handle') + elseif ischar(A) | strcmp(class(A),'function_handle') IA = inv(feval(A,fm,aparams)); else IA = inv(A(fm,aparams)); @@ -167,7 +167,7 @@ end elseif isnumeric(W) B = W; - elseif isstr(W) | strcmp(class(W),'function_handle') + elseif ischar(W) | strcmp(class(W),'function_handle') B = feval(W,mf,aparams); else B = W(mf,aparams); diff --git a/kf_update.m b/kf_update.m index b1bde78..8db3f02 100644 --- a/kf_update.m +++ b/kf_update.m @@ -99,4 +99,3 @@ if nargout > 5 LH = gauss_pdf(y,IM,IS); end - diff --git a/lin_transform.m b/lin_transform.m index 9d0dea6..4d9ec71 100644 --- a/lin_transform.m +++ b/lin_transform.m @@ -58,7 +58,7 @@ if isnumeric(gx) % nop - elseif isstr(gx) | strcmp(class(gx),'function_handle') + elseif ischar(gx) | strcmp(class(gx),'function_handle') gx = feval(gx,M,g_param); else gx = gx(M,g_param); @@ -68,7 +68,7 @@ mu = g*M; elseif isnumeric(g) mu = g; - elseif isstr(g) | strcmp(class(g),'function_handle') + elseif ischar(g) | strcmp(class(g),'function_handle') mu = feval(g,M,g_param); else mu = g(M,g_param); diff --git a/octave_pkg/CITATION b/octave_pkg/CITATION new file mode 100644 index 0000000..5d088ad --- /dev/null +++ b/octave_pkg/CITATION @@ -0,0 +1 @@ +Simo Särkkä (2013). Bayesian Filtering and Smoothing. Cambridge University Press diff --git a/octave_pkg/DESCRIPTION b/octave_pkg/DESCRIPTION new file mode 100644 index 0000000..c2e26c1 --- /dev/null +++ b/octave_pkg/DESCRIPTION @@ -0,0 +1,18 @@ +Name: ekfukf +Version: 1.0.0 +Date: 2017-11-07 +Author: Simo Särkkä , Jouni Hartikainen , Arno Solin +Maintainer: Juan Pablo Carbajal +Title: Kalman filters +Description: EKF/UKF is an optimal filtering toolbox. Optimal filtering is a + frequently used term for a process, in which the state of a + dynamic system is estimated through noisy and indirect measurements. This + toolbox mainly consists of Kalman filters and smoothers, which are the most + common methods used in stochastic state-space estimation. The purpose of the + toolbox is not to provide a highly optimized software package, but instead to + provide a simple framework for building proof-of-concept implementations of + optimal filters and smoothers to be used in practical applications. +Depends: octave (>= 4.0.0) +Autoload: no +License: GPLv2+ +Url: https://github.com/kakila/ekfukf diff --git a/octave_pkg/Makefile b/octave_pkg/Makefile new file mode 100644 index 0000000..649851d --- /dev/null +++ b/octave_pkg/Makefile @@ -0,0 +1,140 @@ +## Copyright 2015-2016 Mike Miller +## Copyright 2015-2016 Carnë Draug +## Copyright 2015-2016 Oliver Heimlich +## Copyright 2017 Juan Pablo Carbajal +## +## Copying and distribution of this file, with or without modification, +## are permitted in any medium without royalty provided the copyright +## notice and this notice are preserved. This file is offered as-is, +## without any warranty. + +## Makefile to simplify Octave Forge package maintenance tasks + +## Some shell programs +MD5SUM ?= md5sum +SED ?= sed +GREP ?= grep +TAR ?= tar + +## Helper function +TOLOWER := $(SED) -e 'y/ABCDEFGHIJKLMNOPQRSTUVWXYZ/abcdefghijklmnopqrstuvwxyz/' + +### Note the use of ':=' (immediate set) and not just '=' (lazy set). +### http://stackoverflow.com/a/448939/1609556 +PACKAGE := $(shell $(SED) -n -e 's/^Name: *\(\w\+\)/\1/p' DESCRIPTION | $(TOLOWER)) +VERSION := $(shell $(SED) -n -e 's/^Version: *\(\w\+\)/\1/p' DESCRIPTION | $(TOLOWER)) +DEPENDS := $(shell $(SED) -n -e 's/^Depends[^,]*, \(.*\)/\1/p' DESCRIPTION | $(SED) 's/ *([^()]*),*/ /g') + +## This are the files that will be created for the releases. +TARGET_DIR := OF +RELEASE_DIR := $(TARGET_DIR)/$(PACKAGE)-$(VERSION) +RELEASE_TARBALL := $(TARGET_DIR)/$(PACKAGE)-$(VERSION).tar.gz +HTML_DIR := $(TARGET_DIR)/$(PACKAGE)-html +HTML_TARBALL := $(TARGET_DIR)/$(PACKAGE)-html.tar.gz + +## Octave binaries +MKOCTFILE ?= mkoctfile +OCTAVE ?= octave --no-gui + +## Remove if not needed, most packages do not have PKG_ADD directives. +M_SOURCES := $(wildcard inst/*.m) + +## Targets that are not filenames. +## https://www.gnu.org/software/make/manual/html_node/Phony-Targets.html +.PHONY: help dist html release install all check run clean + +## make will display the command before runnning them. Use @command +## to not display it (makes specially sense for echo). +help: + @echo "Targets:" + @echo " dist - Create $(RELEASE_TARBALL) for release" + @echo " html - Create $(HTML_TARBALL) for release" + @echo " release - Create both of the above and show md5sums" + @echo + @echo " install - Install the package in GNU Octave" + @echo " all - Build all oct files" + @echo " run - Run Octave with development in PATH (no install)" + @echo " check - Execute package tests (w/o install)" + @echo " doctest - Tests only the help text via the doctest package" + @echo + @echo " clean - Remove releases, html documentation, and oct files" + +# dist and html targets are only PHONY/alias targets to the release +# and html tarballs. +dist: $(RELEASE_TARBALL) +html: $(HTML_TARBALL) + +# An implicit rule with a recipe to build the tarballs correctly. +%.tar.gz: % + tar -c -f - --posix -C "$(TARGET_DIR)/" "$(notdir $<)" | gzip -9n > "$@" + +# Some packages are distributed outside Octave Forge in non tar.gz format. +%.zip: % + cd "$(TARGET_DIR)" ; zip -9qr - "$(notdir $<)" > "$(notdir $@)" + +# Create the unpacked package. +# +# Notes: +# * having this recipe separate from the one that makes the tarball +# makes it easy to have packages in alternative formats (such as zip) +# * note that if a commands needs to be ran in a specific directory, +# the command to "cd" needs to be on the same line. Each line restores +# the original working directory. +$(RELEASE_DIR): + @echo "Creating package version $(VERSION) release ..." + mkdir -p $@ + rsync -a -q --exclude='Makefile' * $@ + +# install is a prerequesite to the html directory (note that the html +# tarball will use the implicit rule for ".tar.gz" files). +$(HTML_DIR): install + @echo "Generating HTML documentation. This may take a while ..." + $(RM) -r "$@" + $(OCTAVE) --no-window-system --silent \ + --eval "pkg load generate_html; " \ + --eval "pkg load $(PACKAGE);" \ + --eval 'generate_package_html ("${PACKAGE}", "$@", "octave-forge");' + chmod -R a+rX,u+w,go-w $@ + +# To make a release, build the distribution and html tarballs. +release: dist html + @$(MD5SUM) $(RELEASE_TARBALL) $(HTML_TARBALL) + @echo "Upload @ https://sourceforge.net/p/octave/package-releases/new/" + +install: $(RELEASE_TARBALL) + @echo "Installing package locally ..." + $(OCTAVE) --silent --eval 'pkg ("install", "-verbose", "$(RELEASE_TARBALL)")' + +clean: + $(RM) -r $(RELEASE_DIR) $(RELEASE_TARBALL) $(HTML_TARBALL) $(HTML_DIR) + +# +# Recipes for testing purposes +# + +# Build any requires oct files. Some packages may not need this at all. +# Other packages may require a configure file to be created and run first. +all: + @echo "all: Nothing to be done" + +# Start an Octave session with the package directories on the path for +# interactice test of development sources. +run: all + $(OCTAVE) --silent --persist --path "inst/" --path "src/" \ + --eval 'if(!isempty("$(DEPENDS)")); pkg load $(DEPENDS); endif;' \ + --eval '$(PKG_ADD)' + +# Test example blocks in the documentation. Needs doctest package +# http://octave.sourceforge.net/doctest/index.html +doctest: all + $(OCTAVE) --path "inst/" --path "src/" \ + --eval '${PKG_ADD}' \ + --eval 'pkg load doctest;' \ + --eval "targets = '$(shell (ls inst; ls src | grep .oct) | cut -f2 -d@ | cut -f1 -d.)';" \ + --eval "targets = strsplit (targets, ' ');" \ + --eval "doctest (targets);" + +# Note "doctest" as prerequesite. When testing the package, also check +# the documentation. +check: all doctest + @echo "No recipies to run tests" diff --git a/octave_pkg/NEWS b/octave_pkg/NEWS new file mode 100644 index 0000000..7959fb0 --- /dev/null +++ b/octave_pkg/NEWS @@ -0,0 +1,8 @@ +Summary of important user-visible changes for releases of the ekfukf package +=============================================================================== +ekfukf-1.0.0 Release Date: 2016-07-11 Release Manager: Juan Pablo Carbajal +=============================================================================== + +** First official release. + +=============================================================================== diff --git a/octave_pkg/do_ekfukf_pkg.sh b/octave_pkg/do_ekfukf_pkg.sh new file mode 100755 index 0000000..89fa297 --- /dev/null +++ b/octave_pkg/do_ekfukf_pkg.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +# COPYING +cp ../LICENSE.txt COPYING +# INDEX +python3 do_index.py +# PACKAGE +python3 do_pkg.py -i .. -o ekfukf_of && \ +# remove created files +rm COPYING INDEX diff --git a/octave_pkg/do_index.py b/octave_pkg/do_index.py new file mode 100644 index 0000000..2db0a28 --- /dev/null +++ b/octave_pkg/do_index.py @@ -0,0 +1,26 @@ + + +with open ('../Contents.m', 'r') as f: + index = f.read().strip() + +import re +m = re.search(r'(?P
.*)%\s$(?P.*)', index, re.S|re.I|re.M) + +header = m.group('header') +index = m.group('content').strip() + +end_re = re.compile (r'^%\s*$', re.M) +index = end_re.split(index) + +topic_re = re.compile (r'^%\s([A-Z][a-z\s]+[a-zA-Z\s]+)$', re.M) +func_re = re.compile (r'^%\s+([A-Z_0-9]+)\s+',re.M) + +with open ('INDEX','w') as outf: + outf.write('ekfukf >> Karmal filters\n') + for section in index: + if section: + topic = topic_re.findall (section) + if topic: + outf.write('{}\n'.format(topic[0])) + funcs = func_re.findall (section) + [outf.write(' {}\n'.format(fn.lower())) for fn in funcs] diff --git a/octave_pkg/do_pkg.py b/octave_pkg/do_pkg.py new file mode 100644 index 0000000..39f527b --- /dev/null +++ b/octave_pkg/do_pkg.py @@ -0,0 +1,146 @@ +''' + Copyright (C) 2017 - Juan Pablo Carbajal + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +''' + +# Author: Juan Pablo Carbajal + +import os, sys, getopt +import shutil +import fnmatch, re + +WD = os.getcwd() + +PKG_STRUCT = { + 'folders': + ['inst', + 'src'], + 'files': + ['COPYING', + 'DESCRIPTION', + 'INDEX', + 'Makefile', + 'NEWS', + 'CITATION'] + } + + +def get_files_byext (files, ext): + ptr = '.*\.{}$'.format(ext) + return [f for f in files if re.match(ptr, f)] + +def analyse_source (source): + content = os.walk (source) + mfiles = dict() + cfiles = dict() + ffiles = dict() + for path, dirn, files in content: + if '.' not in path: + + folder = '.' + if path != source: + folder = '' + while path != source: + path, tmp = os.path.split(path) + folder = os.path.join (tmp,folder) + + tmp = get_files_byext (files, 'm') + if tmp: + mfiles[folder] = tmp + tmp = get_files_byext (files, 'c{1,2}p{0,1}') + if tmp: + cfiles[folder] = tmp + tmp = get_files_byext (files, 'f(90){0,1}') + if tmp: + ffiles[folder] = tmp + + return (mfiles, cfiles, ffiles) + +def do_empty_pkg (dest, src=False): + os.mkdir (dest) + for f in PKG_STRUCT['folders']: + + if (f == 'src') and (not src): + continue + + p = os.path.join (dest, f) + + return + +def put_mfiles (mfiles, source, dest): + print ('Copying m-files to {}'.format(dest)) + + for dirn,files in mfiles.items(): + if dirn == '.': + dirn = '' + + out_dir = os.path.join (dest, 'inst', dirn) + in_files = [os.path.join (source, dirn, f) for f in files] + + if not os.path.exists (out_dir): + os.makedirs (out_dir) + + [shutil.copy(f, out_dir) for f in in_files] + if 'Contents.m' in files: + idx = files.index('Contents.m') + shutil.move (os.path.join (out_dir, 'Contents.m'),\ + os.path.join (out_dir, '__contents__.m')) + + return + +def copy_done_file (in_file, dest): + if os.path.isfile (in_file): + shutil.copy(in_file, dest) + else: + print ('No {} to copy over'.format(in_file)) + return + +def do_pkg (argv): + help_str = 'do_pkg.py -i -o ' + + # Parse input arguments + SOURCE = WD[:] # Source folder + DEST = WD[:] + '_of' # Destination folder + try: + opts, args = getopt.getopt(argv,"hi:o:",["idir=","odir="]) + except getopt.GetoptError: + print (help_str) + sys.exit(2) + for opt, arg in opts: + if opt == '-h': + print (help_str) + sys.exit() + elif opt in ("-i", "--idir"): + SOURCE = os.path.abspath (arg) + elif opt in ("-o", "--odir"): + DEST = os.path.abspath (arg) + print ('Input folder is ', SOURCE) + print ('Output folder is ', DEST) + ###### end parse arguments + + mfiles, cfiles, ffiles = analyse_source (SOURCE) + src_folder = True + if not (cfiles and ffiles): + src_folder = False + do_empty_pkg (DEST, src=src_folder) + + put_mfiles (mfiles, SOURCE, DEST) + + # Pre-generated files + for f in PKG_STRUCT['files']: + copy_done_file (f, DEST) + +if __name__ == '__main__': + do_pkg(sys.argv[1:]); diff --git a/quad_transform.m b/quad_transform.m index e1f5383..ff80536 100644 --- a/quad_transform.m +++ b/quad_transform.m @@ -69,7 +69,7 @@ if isnumeric(gx) % nop - elseif isstr(gx) | strcmp(class(gx),'function_handle') + elseif ischar(gx) | strcmp(class(gx),'function_handle') gx = feval(gx,M,g_param); else gx = gx(M,g_param); @@ -77,7 +77,7 @@ if isnumeric(gxx) % nop - elseif isstr(gxx) | strcmp(class(gxx),'function_handle') + elseif ischar(gxx) | strcmp(class(gxx),'function_handle') gxx = feval(gxx,M,g_param); else gxx = gxx(M,g_param); diff --git a/rk4.m b/rk4.m index 6bf97b5..55780b0 100644 --- a/rk4.m +++ b/rk4.m @@ -153,7 +153,7 @@ % % Chained integration % - if isstr(f) | strcmp(class(f),'function_handle') + if ischar(f) | strcmp(class(f),'function_handle') x1 = x; dx1 = feval(f,x1,Y{1},P1{:}) * dt; x2 = x+0.5*dx1; @@ -173,7 +173,7 @@ dx4 = f(x4,Y{4},P3{:}) * dt; end else - if isstr(f) | strcmp(class(f),'function_handle') + if ischar(f) | strcmp(class(f),'function_handle') x1 = x; dx1 = feval(f,x1,P1{:}) * dt; x2 = x+0.5*dx1; diff --git a/uimm_smooth.m b/uimm_smooth.m index 166750e..25903b1 100644 --- a/uimm_smooth.m +++ b/uimm_smooth.m @@ -130,7 +130,7 @@ if isnumeric(A{i2}) A2 = A{i2}; - elseif isstr(A{i2}) | strcmp(class(A{i2}),'function_handle') + elseif ischar(A{i2}) | strcmp(class(A{i2}),'function_handle') A2 = feval(A{i2},x_bki{i2}(ind{i2}),a_param{i2}); else A2 = A{i2}(x_bki{i2}(ind{i2}),a_param{i2});