├── LICENSE
├── README.md
├── matlab
    ├── examples
    │   ├── car_track.m
    │   ├── check_ders.m
    │   ├── ckf_ws.m
    │   ├── drift_linreg.m
    │   ├── gh_ws.m
    │   ├── pendulum_ckf.m
    │   ├── pendulum_ekf.m
    │   ├── pendulum_ffbs.m
    │   ├── pendulum_ghkf.m
    │   ├── pendulum_iekf.m
    │   ├── pendulum_iplf.m
    │   ├── pendulum_par.m
    │   ├── pendulum_pf.m
    │   ├── pendulum_pf2.m
    │   ├── pendulum_sim.m
    │   ├── pendulum_sim2.m
    │   ├── pendulum_ukf.m
    │   ├── pendulum_ukf5.m
    │   ├── recursive_linreg.m
    │   ├── resampling.m
    │   ├── sut_ws.m
    │   ├── sym_set.m
    │   └── ut5_ws.m
    └── exercises
    │   ├── exercise6_4.m
    │   └── exercise7_2.m
└── python
    ├── common_utilities
        ├── __init__.py
        ├── plot.py
        ├── random.py
        ├── simulate.py
        └── stats.py
    ├── example_notebooks
        ├── __init__.py
        ├── car_track.ipynb
        ├── car_track.json
        ├── pendulum.json
        ├── pendulum_ckf.ipynb
        ├── pendulum_ekf.ipynb
        ├── pendulum_ghkf.ipynb
        ├── pendulum_iekf.ipynb
        ├── pendulum_iplf.ipynb
        ├── pendulum_parameter_est.ipynb
        ├── pendulum_pf.ipynb
        ├── pendulum_pf_cluttered.ipynb
        └── pendulum_ukf.ipynb
    ├── exercises
        ├── Exercise6_4.ipynb
        ├── Exercise7_2.ipynb
        ├── README.md
        └── __init__.py
    ├── requirements.txt
    └── setup.py


/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2023 Simo Särkkä, Lennart Svensson, and Adrien Corenflos
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # [Bayesian-Filtering-and-Smoothing](http://www.cambridge.org/9781108926645)
2 | Companion Matlab and Python codes for the book Bayesian Filtering and Smoothing (2nd ed.) by Simo Särkkä and Lennart Svensson (2023). In addition to Simo Särkkä and Lennart Svensson, Adrien Corenflos has written some of the codes.
3 | 
4 | The codes are all not finished, but they are coming here piece by piece. 
5 | 
6 | [![Binder](http://mybinder.org/badge.svg)](https://beta.mybinder.org/v2/gh/EEA-sensors/Bayesian-Filtering-and-Smoothing/main)
7 | <a href="https://colab.research.google.com/github/EEA-sensors/Bayesian-Filtering-and-Smoothing"><img src="https://colab.research.google.com/img/colab_favicon_256px.png" width="20" height="20"/> Open in colab</a>
8 | 


--------------------------------------------------------------------------------
/matlab/examples/car_track.m:
--------------------------------------------------------------------------------
  1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | %
  3 | % Track car state with Kalman filter and Rauch-Tung-Striebel
  4 | % smoother as in Examples 6.8 and 12.4 of the book
  5 | %
  6 | % Simo Sarkka and Lennart Svensson (2023), Bayesian Filtering and Smoothing,
  7 | % Cambridge University Press. 
  8 | %
  9 | % See LICENSE provided with the software.
 10 | %
 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 12 | 
 13 | %%
 14 | % Set the parameters
 15 | %
 16 |     q = 1;
 17 |     dt = 0.1;
 18 |     s = 0.5;
 19 |     A = [1 0 dt 0;
 20 |         0 1 0 dt;
 21 |         0 0 1 0;
 22 |         0 0 0 1];
 23 |     Q = q*[dt^3/3 0 dt^2/2 0;
 24 |            0 dt^3/3 0 dt^2/2;
 25 |            dt^2/2 0 dt 0;
 26 |            0 dt^2/2 0 dt];
 27 |     
 28 |     H = [1 0 0 0;
 29 |          0 1 0 0];
 30 |     R = s^2*eye(2);
 31 |     m0 = [0;0;1;-1];
 32 |     P0 = eye(4);
 33 | 
 34 | %%
 35 | % Simulate data
 36 | %
 37 | 
 38 |     randn('state',33);  % Use this to get the book's data
 39 | %    rng(14, 'twister');
 40 | 
 41 |     steps = 100;
 42 |     X = zeros(size(A,1),steps);
 43 |     Y = zeros(size(H,1),steps);
 44 |     x = m0;
 45 |     for k=1:steps
 46 |         q = chol(Q)'*randn(size(A,1),1);
 47 |         x = A*x + q;
 48 |         y = H*x + s*randn(2,1);
 49 |         X(:,k) = x;
 50 |         Y(:,k) = y;
 51 |     end
 52 |     
 53 |     plot(X(1,:),X(2,:),'-',Y(1,:),Y(2,:),'.',X(1,1),X(2,1),'*');
 54 |     legend('Trajectory','Measurements');
 55 |     xlabel('{\it x}_1');
 56 |     ylabel('{\it x}_2');
 57 | 
 58 |     % Store the data in JSON
 59 |     %{
 60 |     jstruct = struct('T',T,'X',X,'Y',Y);
 61 |     json = jsonencode(jstruct,'PrettyPrint',true);
 62 |     filename = 'car_track.json';
 63 |     fid = fopen(filename, 'w');
 64 |     fwrite(fid, json);
 65 |     fclose(fid);
 66 |     %}
 67 | 
 68 | %%
 69 | % Kalman filter
 70 | %
 71 |     m = m0;
 72 |     P = P0;
 73 |     kf_m = zeros(size(m,1),size(Y,2));
 74 |     kf_P = zeros(size(P,1),size(P,2),size(Y,2));
 75 |     for k=1:size(Y,2)
 76 |         m = A*m;
 77 |         P = A*P*A' + Q;
 78 |         
 79 |         S = H*P*H' + R;
 80 |         K = P*H'/S;
 81 |         m = m + K*(Y(:,k) - H*m);
 82 |         P = P - K*S*K';
 83 |         
 84 |         kf_m(:,k) = m;
 85 |         kf_P(:,:,k) = P;
 86 |     end
 87 |     
 88 |     rmse_raw = sqrt(mean(sum((Y - X(1:2,:)).^2,1)))
 89 |     rmse_kf = sqrt(mean(sum((kf_m(1:2,:) - X(1:2,:)).^2,1)))
 90 |     
 91 |     clf;
 92 |     h=plot(X(1,:),X(2,:),'-',Y(1,:),Y(2,:),'o',...
 93 |         kf_m(1,:),kf_m(2,:),'-');
 94 |     legend('True Trajectory','Measurements','Filter Estimate');
 95 |     xlabel('{\it x}_1');
 96 |     ylabel('{\it x}_2');
 97 | 
 98 | %%
 99 | % RTS smoother
100 | %
101 |     ms = m;
102 |     Ps = P;
103 |     rts_m = zeros(size(m,1),size(Y,2));
104 |     rts_P = zeros(size(P,1),size(P,2),size(Y,2));
105 |     rts_m(:,end) = ms;
106 |     rts_P(:,:,end) = Ps;
107 |     for k=size(kf_m,2)-1:-1:1
108 |         mp = A*kf_m(:,k);
109 |         Pp = A*kf_P(:,:,k)*A'+Q;
110 |         Gk = kf_P(:,:,k)*A'/Pp; 
111 |         ms = kf_m(:,k) + Gk*(ms - mp);
112 |         Ps = kf_P(:,:,k) + Gk*(Ps - Pp)*Gk';
113 |         rts_m(:,k) = ms;
114 |         rts_P(:,:,k) = Ps;
115 |     end
116 |     
117 |     rmse_rts = sqrt(mean(sum((rts_m(1:2,:) - X(1:2,:)).^2,1)))
118 |     
119 |     clf;
120 |     h=plot(X(1,:),X(2,:),'-',Y(1,:),Y(2,:),'o',...
121 |         rts_m(1,:),rts_m(2,:),'-');
122 |     legend('True Trajectory','Measurements','Smoother Estimate');
123 |     xlabel('{\it x}_1');
124 |     ylabel('{\it x}_2');
125 | 
126 |     


--------------------------------------------------------------------------------
/matlab/examples/check_ders.m:
--------------------------------------------------------------------------------
  1 | % [Fx2,Fxx2] = check_ders(x,f,Fx,Fxx,h)
  2 | %
  3 | % Check that the analytical first (and probably second)
  4 | % derivative of the function f matches a numerically computed one.
  5 | 
  6 | function [Fx2,Fxx2] = check_ders(x,f,Fx,Fxx,h)
  7 |     if nargin < 4
  8 |        Fxx = {};
  9 |     end
 10 |     if nargin < 5
 11 |        h = [];
 12 |     end
 13 | 
 14 |     if isempty(h)
 15 |         h = 1e-6;
 16 |     end
 17 | 
 18 |     thrs = [1e-6 1e-4 1e-2 Inf];
 19 |     strs = {'looks great','quite fine','barely passes','failed'};
 20 |     
 21 |     n = size(x,1);
 22 | 
 23 |     %
 24 |     % Evaluate the function
 25 |     %
 26 |     f0 = f(x);
 27 |     d = length(f0);
 28 | 
 29 |     fprintf('** Checking derivative of %d -> %d function:\n',n,d);
 30 |     disp(f);
 31 |     
 32 |     %
 33 |     % Evaluate and check Fx
 34 |     %    
 35 |     if ~isnumeric(Fx)
 36 |       Fx1 = Fx(x);
 37 |     else
 38 |       Fx1 = Fx;
 39 |     end
 40 |     
 41 |     Fx2 = zeros(size(Fx1));
 42 |     
 43 |     for i=1:n
 44 |         dx = zeros(size(x));
 45 |         dx(i) = h;
 46 |         Fx2(:,i) = (f(x + dx) - f0) / h;
 47 |     end
 48 |     
 49 |     err = norm(Fx1 - Fx2);
 50 |     ind = find(err < thrs, 1);
 51 |         
 52 |     fprintf('Checking Fx, error %g, %s.\n',err,strs{ind});
 53 |     
 54 |     if ind == length(thrs)
 55 |         analytical = Fx1
 56 |         numerical  = Fx2
 57 |         error('Derivative check for Fx failed.');
 58 |     end
 59 | 
 60 |     if ~isempty(Fxx)
 61 |         %
 62 |         % Check Fxx
 63 |         %
 64 |         if isa(Fxx,'function_handle')
 65 |             Fxx1 = Fxx(x);
 66 |             Fxx2 = Fxx1;
 67 |         else        
 68 |             Fxx1 = cell(1,d);
 69 |             Fxx2 = cell(1,d);
 70 |             for i=1:d
 71 |                 if ~isnumeric(Fxx{i})
 72 |                     Fxx1{i} = Fxx{i}(x);
 73 |                 else
 74 |                     Fxx1{i} = Fxx{i};
 75 |                 end
 76 |                 Fxx2{i} = zeros(n);
 77 |             end
 78 |         end
 79 |         
 80 |         if ~isnumeric(Fx)
 81 |             Fx0 = Fx(x);
 82 |         else
 83 |             Fx0 = Fx;
 84 |         end
 85 |         
 86 |         for i=1:n 
 87 |             dx = zeros(size(x));
 88 |             dx(i) = h;
 89 |             if ~isnumeric(Fx)
 90 |                 dFx_dxi = (Fx(x + dx) - Fx0) / h;
 91 |             else
 92 |                 dFx_dxi = (Fx2 - Fx0) / h; % todo replace with: df2 = @(x) ( f(x+2*h) - 2*f(x+h) + f(x))/h^2;
 93 |             end
 94 |             for j=1:d
 95 |                 Fxx2{j}(i,:) = dFx_dxi(j,:);
 96 |             end
 97 |         end
 98 |         
 99 |         for i=1:d
100 |             err = norm(Fxx1{i} - Fxx2{i});
101 |             ind = find(err < thrs, 1);
102 |         
103 |             fprintf('Checking Fxx{%d}, error %g, %s.\n',i,err,strs{ind});
104 |     
105 |             if ind == length(thrs)
106 |                 analytical = Fxx1{i}
107 |                 numerical  = Fxx2{i}
108 |                 error('Derivative check for Fxx failed.');
109 |             end
110 |         end
111 |     else
112 |         Fxx2 = {};
113 |     end
114 | end
115 | 


--------------------------------------------------------------------------------
/matlab/examples/ckf_ws.m:
--------------------------------------------------------------------------------
1 | % [W, XI] = ckf_ws(n)
2 | %
3 | % Compute spherical cubature weights and unit sigma points.
4 | %
5 | function [W, XI] = ckf_ws(n)
6 |     XI = sqrt(n) * [eye(n) -eye(n)];
7 |     W  = ones(1,2*n)/(2*n);
8 | end
9 | 


--------------------------------------------------------------------------------
/matlab/examples/drift_linreg.m:
--------------------------------------------------------------------------------
  1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | %
  3 | % Drifted linear regression demonstration from Chapter 3 of the book
  4 | %
  5 | % Simo Sarkka and Lennart Svensson (2023), Bayesian Filtering and Smoothing,
  6 | % 2nd ed. Cambridge University Press. 
  7 | %
  8 | % See LICENSE provided with the software.
  9 | %
 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 11 | 
 12 | 
 13 | %%
 14 | % Simulate data
 15 | %
 16 | 
 17 | %    randn('state',12);  % Use this to get book's data
 18 |     rng(1, 'twister');
 19 |     
 20 |     dt = 0.01;
 21 |     sd = 0.2;
 22 |     t = (0:dt:2);
 23 |     x = sin(pi*t);
 24 |     y = x + sd*randn(size(x));
 25 |     
 26 |     h = plot(t,y,'.',t,x,'-');
 27 |     
 28 |     axis([0 2 -1.5 1.5]);
 29 |     
 30 |     set(h,'Markersize',7);
 31 |     set(h,'LineWidth',3);
 32 |     set(h(1),'Color',[0.0 0.0 0.0]);
 33 |     set(h(2),'Color',[0.7 0.7 0.7]);
 34 |     
 35 |     
 36 |     h = legend('Measurements','True Signal','Location','NE');
 37 |     xlabel('{\it t}');
 38 |     
 39 |     rmse = sqrt(mean((x - y).^2))
 40 |   
 41 | %%
 42 | % Kalman filter 1
 43 | %
 44 |     m0 = [0;0];
 45 |     P0 = eye(2);
 46 |     m = m0;
 47 |     P = P0;
 48 |     kf1_MM = zeros(size(m0,1),length(y));
 49 |     kf1_PP = zeros(size(P0,1),size(P0,1),length(y));
 50 |     Q = 0.1*eye(2)*dt;
 51 |     for k=1:length(y)
 52 |         P = P + Q;
 53 |         H = [1 t(k)];
 54 |         S = H*P*H'+sd^2;
 55 |         K = P*H'/S;
 56 |         m = m + K*(y(k)-H*m);
 57 |         P = P - K*S*K';
 58 |         
 59 |         kf1_MM(:,k) = m;
 60 |         kf1_PP(:,:,k) = P;
 61 |     end
 62 |     
 63 |     est = kf1_MM(1,:) + t.*kf1_MM(2,:);
 64 |     h = plot(t,y,'.',t,x,'-',t,est,'b-');
 65 |     
 66 |     axis([0 2 -1.5 1.5]);
 67 |     
 68 |     set(h,'Markersize',7);
 69 |     set(h(2),'LineWidth',4);
 70 |     set(h(3),'LineWidth',1.5);
 71 |     set(h(1),'Color',[0.0 0.0 0.0]);
 72 |     set(h(2),'Color',[0.7 0.7 0.7]);
 73 |     set(h(3),'Color',[0.0 0.0 0.0]);
 74 |  
 75 |     h = legend('Measurements','True Signal','Estimate','Location','NE');
 76 |     
 77 |     xlabel('{\it t}');
 78 |     
 79 |     rmse = sqrt(mean((x - est).^2))
 80 | 
 81 | %%
 82 | % Kalman filter 2
 83 | %
 84 |     m0 = [0;0];
 85 |     P0 = eye(2);
 86 |     m = m0;
 87 |     P = P0;
 88 |     kf2_MM = zeros(size(m0,1),length(y));
 89 |     kf2_PP = zeros(size(P0,1),size(P0,1),length(y));
 90 |     q = 10;
 91 |     A = [1 dt; 0 1];
 92 |     Q = q * [dt^3/3 dt^2/2; dt^3/3 dt];
 93 |     
 94 |     for k=1:length(y)
 95 |         m = A*m;
 96 |         P = A*P*A' + Q;      
 97 |         H = [1 0];
 98 |         S = H*P*H'+sd^2;
 99 |         K = P*H'/S;
100 |         m = m + K*(y(k)-H*m);
101 |         P = P - K*S*K';
102 |         
103 |         kf2_MM(:,k) = m;
104 |         kf2_PP(:,:,k) = P;
105 |     end
106 |     
107 |     est = kf2_MM(1,:);
108 |     h = plot(t,y,'.',t,x,'-',t,est,'b-');
109 |     
110 |     axis([0 2 -1.5 1.5]);
111 |     
112 |     set(h,'Markersize',7);
113 |     set(h(2),'LineWidth',4);
114 |     set(h(3),'LineWidth',1.5);
115 |     set(h(1),'Color',[0.0 0.0 0.0]);
116 |     set(h(2),'Color',[0.7 0.7 0.7]);
117 |     set(h(3),'Color',[0.0 0.0 0.0]);
118 |     
119 |     
120 |     h = legend('Measurements','True Signal','Estimate','Location','NE');
121 |     
122 |     xlabel('{\it t}');
123 |     
124 |     rmse = sqrt(mean((x - est).^2))
125 | 
126 | %%
127 | % Kalman smoother
128 | %
129 |     ms = kf2_MM(:,end);
130 |     Ps = kf2_PP(:,:,end);
131 |     rts_m = zeros(size(m,1),length(y));
132 |     rts_P = zeros(size(P,1),size(P,2),length(y));
133 |     rts_m(:,end) = ms;
134 |     rts_P(:,:,end) = Ps;
135 |     for k=size(kf2_MM,2)-1:-1:1
136 |         mp = A*kf2_MM(:,k);
137 |         Pp = A*kf2_PP(:,:,k)*A'+Q;
138 |         Ck = kf2_PP(:,:,k)*A'/Pp;
139 |         ms = kf2_MM(:,k) + Ck*(ms - mp);
140 |         Ps = kf2_PP(:,:,k) + Ck*(Ps - Pp)*Ck';
141 |         rts_m(:,k) = ms;
142 |         rts_P(:,:,k) = Ps;
143 |     end
144 |     
145 |     est = rts_m(1,:);
146 |     h = plot(t,y,'.',t,x,'-',t,est,'b-');
147 |     
148 |     axis([0 2 -1.5 1.5]);
149 |     
150 |     set(h,'Markersize',7);
151 |     set(h(2),'LineWidth',4);
152 |     set(h(3),'LineWidth',1.5);
153 |     set(h(1),'Color',[0.0 0.0 0.0]);
154 |     set(h(2),'Color',[0.7 0.7 0.7]);
155 |     set(h(3),'Color',[0.0 0.0 0.0]);
156 |     
157 |     
158 |     h = legend('Measurements','True Signal','Estimate','Location','NE');
159 |     
160 |     xlabel('{\it t}');
161 |     
162 |     rmse = sqrt(mean((x - est).^2))
163 |     
164 | %%
165 | % Prediction
166 | %
167 |     psteps = 50;
168 |     
169 |     PMM = zeros(size(rts_m,1),size(rts_m,2)+psteps);
170 |     PPP = zeros(size(rts_P,1),size(rts_P,2),size(rts_P,3)+psteps);
171 |     ind = size(rts_m,2);
172 |     PMM(:,1:ind) = rts_m;
173 |     PPP(:,:,1:ind) = rts_P;
174 |     
175 |     PT = [t t(end)+(1:psteps)*dt];
176 |     m = rts_m(:,end);
177 |     P = rts_P(:,:,end);
178 |     for k=ind+1:ind+psteps
179 |         m = A*m;
180 |         P = A*P*A' + Q;
181 |         PMM(:,k) = m;
182 |         PPP(:,:,k) = P;    
183 |     end
184 |     
185 |     est = PMM(1,:);
186 |     h = plot(t,y,'.',t,x,'-',PT,est,'b-');
187 |     
188 |     axis([0 PT(end) -1.5 1.5]);
189 |     
190 |     set(h,'Markersize',7);
191 |     set(h(2),'LineWidth',4);
192 |     set(h(3),'LineWidth',1.5);
193 |     set(h(1),'Color',[0.0 0.0 0.0]);
194 |     set(h(2),'Color',[0.7 0.7 0.7]);
195 |     set(h(3),'Color',[0.0 0.0 0.0]);
196 |     
197 |     h = legend('Measurements','True Signal','Estimate','Location','SW');
198 |     
199 |     xlabel('{\it t}');
200 | 


--------------------------------------------------------------------------------
/matlab/examples/gh_ws.m:
--------------------------------------------------------------------------------
 1 | % Syntax:
 2 | %   [W,XI] = gh_ws(n,p)
 3 | %
 4 | % Description:
 5 | %   Generate Gauss-Hermite cubature order p based
 6 | %   weights W and sigma points SX such that
 7 | %
 8 | %     int g(x) N(x | 0,I) dx =~ sum W(i) g(SX(i))
 9 | 
10 | function [ W, XI, xi1, W1 ] = gh_ws(n,p)
11 | 
12 |     %
13 |     % Form Probabilists' Hermite polynomials of
14 |     % order p-1 and p
15 |     %
16 |     Hpm = 1;
17 |     Hp  = [1 0];
18 |     for i=1:p-1
19 |         tmp = Hp;
20 |         Hp = [Hp 0] - [0 0 i*Hpm];
21 |         Hpm = tmp;
22 |     end
23 | 
24 |     %
25 |     % Single dimensional weights and points
26 |     %
27 |     xi1 = roots(Hp)';
28 |     W1 = factorial(p)./(p^2*polyval(Hpm,xi1).^2);
29 |      
30 |     %Generate all p^n collections of indexes by
31 |     %transforming numbers 0...p^n-1) into p-base system
32 |     %and by adding 1 to each digit
33 |     num = 0:(p^n-1);
34 |     ind = zeros(n,p^n);
35 |     for i=1:n
36 |         ind(i,:) = rem(num,p)+1;
37 |         num = floor(num / p);
38 |     end
39 | 
40 |     %Form the unit sigma points and weights
41 |     XI = xi1(ind);
42 |     W  = prod(W1(ind),1); % ND weights
43 | 


--------------------------------------------------------------------------------
/matlab/examples/pendulum_ckf.m:
--------------------------------------------------------------------------------
  1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | %
  3 | % Estimate pendulum state with CKF and CRTS as in Examples 8.13 and 14.7 of
  4 | % the book
  5 | %
  6 | % Simo Sarkka and Lennart Svensson (2023), Bayesian Filtering and Smoothing,
  7 | % 2nd ed., Cambridge University Press.
  8 | % 
  9 | % See LICENSE provided with the software.
 10 | %
 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 12 | 
 13 | %%
 14 | % Simulate data
 15 | %
 16 |     pendulum_sim;
 17 | 
 18 | %%
 19 | % Filter
 20 | %
 21 | 
 22 |     m = m0;
 23 |     P = P0;
 24 |     
 25 |     n = size(m,1);
 26 | 
 27 |     XI = sqrt(n) * [eye(n) -eye(n)];
 28 |     W  = ones(1,2*n)/(2*n);
 29 |     
 30 |     %
 31 |     % Do the filtering
 32 |     %  
 33 |     MM = zeros(size(m,1),length(Y));
 34 |     PP = zeros(size(P,1),size(P,2),length(Y));
 35 |     for k=1:length(Y)
 36 | 
 37 |         % Form the sigma points for dynamic model
 38 |         SX = repmat(m,1,size(XI,2)) + chol(P,'lower') * XI;
 39 | 
 40 |         % Propagate through the dynamic model
 41 |         HX = [SX(1,:)+SX(2,:)*DT; SX(2,:)-g*sin(SX(1,:))*DT];
 42 |   
 43 |         % Compute the predicted mean and covariance
 44 |         m = zeros(size(m));
 45 |         P = zeros(size(P));
 46 |         for i=1:size(HX,2)
 47 |             m = m + W(i) * HX(:,i);
 48 |         end
 49 |         for i=1:size(HX,2)
 50 |             P = P + W(i) * (HX(:,i) - m) * (HX(:,i) - m)';
 51 |         end
 52 |         P = P + Q;
 53 | 
 54 |         % Form sigma points for measurement step and
 55 |         % propagate throught the measurement model
 56 |         SX = repmat(m,1,size(XI,2)) + chol(P,'lower') * XI;
 57 |         HY = sin(SX(1,:)); 
 58 |     
 59 |         % Compute the updated mean and covariance
 60 |         mu = zeros(size(HY,1),1);
 61 |         S  = zeros(size(HY,1),size(HY,1));
 62 |         C  = zeros(size(SX,1),size(HY,1));
 63 |         for i=1:size(SX,2)
 64 |             mu = mu + W(i) * HY(:,i);
 65 |         end
 66 |         for i=1:size(SX,2)
 67 |             S = S + W(i) * (HY(:,i) - mu) * (HY(:,i) - mu)';
 68 |             C = C + W(i) * (SX(:,i) - m) * (HY(:,i) - mu)';
 69 |         end
 70 |         S = S + R;
 71 |     
 72 |         % Compute the gain and updated mean and covariance  
 73 |         K = C/S;
 74 |         m = m + K*(Y(k) - mu);
 75 |         P = P - K*S*K';
 76 |     
 77 |         MM(:,k) = m;
 78 |         PP(:,:,k) = P;
 79 |     end  
 80 |   
 81 |     h = plot(T,Y,'k.',T,X(1,:),'r-',T,MM(1,:),'b--');
 82 |     set(h,'Linewidth',5);
 83 |     title('CKF estimate');
 84 |     legend('Measurements','True','Estimate');
 85 |   
 86 |     rmse_ckf = sqrt(mean((X(1,:)-MM(1,:)).^2))
 87 | 
 88 | %%
 89 | % Smoother
 90 | %
 91 | 
 92 |     ms = m;
 93 |     Ps = P;
 94 |     MS = zeros(size(m,1),length(Y));
 95 |     PS = zeros(size(P,1),size(P,2),length(Y));
 96 |     MMS(:,k) = m;
 97 |     PPS(:,:,k) = P;
 98 |     for k=size(MM,2)-1:-1:1
 99 |         m = MM(:,k);
100 |         P = PP(:,:,k);
101 |     
102 |         % Form the sigma points for dynamic model
103 |         SX = repmat(m,1,size(XI,2)) + chol(P,'lower') * XI;
104 | 
105 |         % Propagate through the dynamic model
106 |         HX = [SX(1,:)+SX(2,:)*DT; SX(2,:)-g*sin(SX(1,:))*DT];
107 |  
108 |         % Compute the predicted mean and covariance
109 |         % and the cross-covariance D.
110 |         mp = zeros(size(m));
111 |         Pp = zeros(size(P));
112 |         D  = zeros(size(P));
113 |         for i=1:size(HX,2)
114 |             mp = mp + W(i) * HX(:,i);
115 |         end
116 |         for i=1:size(HX,2)
117 |             Pp = Pp + W(i) * (HX(:,i) - mp) * (HX(:,i) - mp)';
118 |             D = D + W(i) * (SX(:,i) - m) * (HX(:,i) - mp)';
119 |         end
120 |         Pp = Pp + Q;
121 | 
122 |         Gk = D/Pp;
123 |         ms = m + Gk*(ms - mp);
124 |         Ps = P + Gk*(Ps - Pp)*Gk';
125 |         MMS(:,k) = ms;
126 |         PPS(:,:,k) = Ps;
127 |     end
128 |   
129 |     h = plot(T,Y,'k.',T,X(1,:),'r-',...
130 |              T,MM(1,:),'b--',T,MMS(1,:),'g--');
131 |     set(h,'Linewidth',5);
132 |     title('CKF and CRTS estimates');
133 |     legend('Measurements','True','CKF','CRTS');
134 |     
135 |     rmse_crts = sqrt(mean((X(1,:)-MMS(1,:)).^2))
136 |     
137 | %%
138 | % Plot the filtering result
139 | %
140 | 
141 |     clf;
142 |     h=plot(T,X(1,:),'k',T,Y,'bo',T,MM(1,:),'r');
143 |   
144 |     legend('True angle','Measurements','CKF estimate');
145 |     xlabel('Time{\it t}');
146 |     ylabel('Pendulum angle {\it{x}}_{1,{\it{k}}}') 
147 |     
148 | 
149 | %%
150 | % Plot the smoothing result
151 | %
152 | 
153 |     clf;
154 |     h=plot(T,X(1,:),'k',T,Y,'bo',T,MMS(1,:),'r');
155 |   
156 |     legend('True angle','Measurements','CRTSS estimate');
157 |     xlabel('Time{\it t}');
158 |     ylabel('Pendulum angle {\it{x}}_{1,{\it{k}}}') 
159 |    
160 |    
161 | 


--------------------------------------------------------------------------------
/matlab/examples/pendulum_ekf.m:
--------------------------------------------------------------------------------
  1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | %
  3 | % Estimate pendulum state with EKF and ERTS as in Examples 7.6 and 13.2 of
  4 | % the book
  5 | %
  6 | % Simo Sarkka and Lennart Svensson (2023), Bayesian Filtering and Smoothing,
  7 | % 2nd ed., Cambridge University Press.
  8 | % 
  9 | % See LICENSE provided with the software.
 10 | %
 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 12 |     
 13 | %%
 14 | % Simulate data
 15 | %
 16 |     pendulum_sim;
 17 | 
 18 | %%
 19 | % Check the derivatives that we have computed
 20 | %
 21 |     f_fun = @(m) [m(1)+m(2)*DT; m(2)-g*sin(m(1))*DT];
 22 |     F_fun = @(m) [1 DT; -g*cos(m(1))*DT 1];
 23 | 
 24 |     x = randn(2,1);
 25 |     check_ders(x, f_fun, F_fun);
 26 | 
 27 |     h_fun = @(m) sin(m(1));
 28 |     H_fun = @(m) [cos(m(1)) 0];
 29 |     check_ders(x, h_fun, H_fun);
 30 | 
 31 | 
 32 | %%
 33 | % Filter
 34 | %
 35 | 
 36 |     m = m0;
 37 |     P = P0;
 38 |     MM = zeros(size(m,1),length(Y));
 39 |     PP = zeros(size(P,1),size(P,2),length(Y));
 40 |     for k=1:length(Y)
 41 |         f = [m(1)+m(2)*DT; m(2)-g*sin(m(1))*DT];
 42 |         F = [1 DT; -g*cos(m(1))*DT 1];
 43 |         m = f;
 44 |         P = F*P*F' + Q;
 45 |     
 46 |         h = sin(m(1));
 47 |         H = [cos(m(1)) 0];
 48 |         S = H*P*H' + R;
 49 |         K = P*H'/S;
 50 |         m = m + K*(Y(k) - h);
 51 |         P = P - K*S*K';
 52 |     
 53 |         MM(:,k) = m;
 54 |         PP(:,:,k) = P;
 55 |     end
 56 |   
 57 |     subplot(2,1,1);
 58 |     h = plot(T,Y,'k.',T,X(1,:),'r-',T,MM(1,:),'b--');
 59 |     set(h,'Linewidth',5);
 60 |     title('EKF estimate');
 61 |     legend('Measurements','True','Estimate');
 62 | 
 63 |     subplot(2,1,2);
 64 |     h = plot(T,squeeze(PP(1,1,:)),'b--');
 65 |     
 66 |     
 67 |     rmse_ekf = sqrt(mean((X(1,:)-MM(1,:)).^2))
 68 |   
 69 | %%
 70 | % Smoother
 71 | %
 72 | 
 73 |     ms = m;
 74 |     Ps = P;
 75 |     MMS = zeros(size(m,1),length(Y));
 76 |     PPS = zeros(size(P,1),size(P,2),length(Y));
 77 |     MMS(:,end) = m;
 78 |     PPS(:,:,end) = P;
 79 |     for k=size(MM,2)-1:-1:1
 80 |         m = MM(:,k);
 81 |         P = PP(:,:,k);
 82 |         f = [m(1)+m(2)*DT; m(2)-g*sin(m(1))*DT];
 83 |         F = [1 DT; -g*cos(m(1))*DT 1];
 84 |     
 85 |         mp = f;
 86 |         Pp = F*P*F'+Q;
 87 |         Gk = P*F'/Pp;
 88 |         ms = m + Gk*(ms - mp);
 89 |         Ps = P + Gk*(Ps - Pp)*Gk';
 90 |         MMS(:,k) = ms;
 91 |         PPS(:,:,k) = Ps;
 92 |     end
 93 | 
 94 |     h = plot(T,Y,'k.',T,X(1,:),'r-',...
 95 |              T,MM(1,:),'b--',T,MMS(1,:),'g--');
 96 |     set(h,'Linewidth',5);
 97 |     title('EKF and ERTS estimates');
 98 |     legend('Measurements','True','EKF','ERTS');
 99 |     
100 |     rmse_erts = sqrt(mean((X(1,:)-MMS(1,:)).^2))
101 | 
102 | %%
103 | % Plot the filtering result
104 | %
105 | 
106 |     clf;
107 |     h=plot(T,X(1,:),'k',T,Y,'bo',T,MM(1,:),'r');
108 |   
109 |     legend('True angle','Measurements','EKF estimate');
110 |     xlabel('Time{\it t}');
111 |     ylabel('Pendulum angle {\it{x}}_{1,{\it{k}}}') 
112 |     
113 | 
114 | %%
115 | % Plot the smoothing result
116 | %
117 | 
118 |     clf;
119 |     h=plot(T,X(1,:),'k',T,Y,'bo',T,MMS(1,:),'r');
120 |   
121 |     legend('True angle','Measurements','ERTSS estimate');
122 |     xlabel('Time{\it t}');
123 |     ylabel('Pendulum angle {\it{x}}_{1,{\it{k}}}') 
124 |     


--------------------------------------------------------------------------------
/matlab/examples/pendulum_ffbs.m:
--------------------------------------------------------------------------------
 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 2 | %
 3 | % Estimate pendulum state with a particle smoother as in Example 15.3 of
 4 | % the book
 5 | %
 6 | % Simo Sarkka and Lennart Svensson (2023), Bayesian Filtering and Smoothing,
 7 | % 2nd ed., Cambridge University Press.
 8 | % 
 9 | % See LICENSE provided with the software.
10 | %
11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
12 | 
13 | %%
14 | % Run the filter
15 | %
16 |     pendulum_pf;
17 | 
18 | %%
19 | % Backward simulation smoother
20 | %
21 | 
22 |     % You can comment this out
23 |     rng(1, 'twister');
24 | 
25 |     NS = 100;
26 |     SM_SSX = zeros(size(m,1),NS,length(Y));
27 | 
28 |     fprintf('Doing backward simulation which might take time...\n');
29 | 
30 |     tic
31 |   
32 |     for i=1:NS
33 |         ind = floor(rand * N + 1);
34 |         xn = SSX(:,ind,end);
35 |         SM_SSX(:,i,end) = xn;
36 |         for k=length(Y)-1:-1:1
37 |             SX = SSX(:,:,k);
38 |             mu = [SX(1,:)+SX(2,:)*DT; SX(2,:)-g*sin(SX(1,:))*DT];
39 |             
40 |             % Evaluate the transition density
41 |             DX = xn-mu;
42 |             E = 0.5 * sum(DX .* (Q \ DX),1) + 0.5 * size(mu,1) * log(2*pi) + 0.5 * log(det(Q));
43 |             W = exp(-E);
44 |             W  = W ./ sum(W);
45 | 
46 |             % Draw a new sample
47 |             ind = resampling(W,'multinomial',1);
48 |             xn = SSX(:,ind,k);
49 |             
50 |             SM_SSX(:,i,k) = xn;
51 |         end
52 |         if rem(i,10)==0
53 |             plot(T,X(1,:),'r-',T,squeeze(SM_SSX(1,i,:)),'b--');
54 |             title(sprintf('Doing backward simulation %d/%d\n',i,NS));
55 |             drawnow;
56 |         end
57 |     end
58 |     
59 |     toc
60 | 
61 |     MMS = squeeze(mean(SM_SSX,2));
62 |   
63 |     h = plot(T,Y,'k.',T,X(1,:),'r-',T,MMS(1,:),'b--');
64 |     set(h,'Linewidth',5);
65 |     fprintf('FFBS estimate.\n');
66 |     legend('Measurements','True','Estimate');
67 |   
68 |     rmse_ffbs = sqrt(mean((X(1,:)-MMS(1,:)).^2))
69 |   
70 | %%
71 | % Plot the final smoothing result
72 | %
73 | 
74 |     clf;
75 |     h=plot(T,X(1,:),'k',T,Y,'bo',T,MMS(1,:),'r');
76 |     xlabel('Time{\it t}');
77 |     ylabel('Pendulum angle {\it{x}}_{1,{\it{k}}}') 
78 |     legend('True angle','Measurements','FFBS estimate');
79 |     
80 | 


--------------------------------------------------------------------------------
/matlab/examples/pendulum_ghkf.m:
--------------------------------------------------------------------------------
  1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | %
  3 | % Estimate pendulum state with GHKF and GHRTS as in Examples 8.8 and 14.4 of
  4 | % the book
  5 | %
  6 | % Simo Sarkka and Lennart Svensson (2023), Bayesian Filtering and Smoothing,
  7 | % 2nd ed., Cambridge University Press.
  8 | % 
  9 | % See LICENSE provided with the software.
 10 | %
 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 12 | 
 13 | %%
 14 | % Simulate data
 15 | %
 16 |     pendulum_sim;
 17 | 
 18 | %%
 19 | % Filter
 20 | %
 21 | 
 22 |     m = m0;
 23 |     P = P0;
 24 |     
 25 |     p = 5; % Order of the method
 26 |     n = size(m,1);
 27 | 
 28 |     %
 29 |     % Form Probabilists' Hermite polynomials of
 30 |     % order p-1 and p
 31 |     %
 32 |     Hpm = 1;
 33 |     Hp  = [1 0];
 34 |     for i=1:p-1
 35 |         tmp = Hp;
 36 |         Hp = [Hp 0] - [0 0 i*Hpm];
 37 |         Hpm = tmp;
 38 |     end
 39 | 
 40 |     %
 41 |     % Single dimensional weights and points
 42 |     %
 43 |     xi1 = roots(Hp)';
 44 |     W1 = factorial(p)./(p^2*polyval(Hpm,xi1).^2);
 45 |      
 46 |     %Generate all p^n collections of indexes by
 47 |     %transforming numbers 0...p^n-1) into p-base system
 48 |     %and by adding 1 to each digit
 49 |     num = 0:(p^n-1);
 50 |     ind = zeros(n,p^n);
 51 |     for i=1:n
 52 |         ind(i,:) = rem(num,p)+1;
 53 |         num = floor(num / p);
 54 |     end
 55 | 
 56 |     %Form the unit sigma points and weights
 57 |     XI = xi1(ind);
 58 |     W = prod(W1(ind),1); % ND weights
 59 |     
 60 |     %
 61 |     % Do the filtering
 62 |     %  
 63 |     MM = zeros(size(m,1),length(Y));
 64 |     PP = zeros(size(P,1),size(P,2),length(Y));
 65 |     for k=1:length(Y)
 66 | 
 67 |         % Form the sigma points for dynamic model
 68 |         SX = repmat(m,1,size(XI,2)) + chol(P,'lower') * XI;
 69 | 
 70 |         % Propagate through the dynamic model
 71 |         HX = [SX(1,:)+SX(2,:)*DT; SX(2,:)-g*sin(SX(1,:))*DT];
 72 |   
 73 |         % Compute the predicted mean and covariance
 74 |         m = zeros(size(m));
 75 |         P = zeros(size(P));
 76 |         for i=1:size(HX,2)
 77 |             m = m + W(i) * HX(:,i);
 78 |         end
 79 |         for i=1:size(HX,2)
 80 |             P = P + W(i) * (HX(:,i) - m) * (HX(:,i) - m)';
 81 |         end
 82 |         P = P + Q;
 83 | 
 84 |         % Form sigma points for measurement step and
 85 |         % propagate throught the measurement model
 86 |         SX = repmat(m,1,size(XI,2)) + chol(P,'lower') * XI;
 87 |         HY = sin(SX(1,:)); 
 88 |     
 89 |         % Compute the updated mean and covariance
 90 |         mu = zeros(size(HY,1),1);
 91 |         S  = zeros(size(HY,1),size(HY,1));
 92 |         C  = zeros(size(SX,1),size(HY,1));
 93 |         for i=1:size(SX,2)
 94 |             mu = mu + W(i) * HY(:,i);
 95 |         end
 96 |         for i=1:size(SX,2)
 97 |             S = S + W(i) * (HY(:,i) - mu) * (HY(:,i) - mu)';
 98 |             C = C + W(i) * (SX(:,i) - m) * (HY(:,i) - mu)';
 99 |         end
100 |         S = S + R;
101 |     
102 |         % Compute the gain and updated mean and covariance  
103 |         K = C/S;
104 |         m = m + K*(Y(k) - mu);
105 |         P = P - K*S*K';
106 |     
107 |         MM(:,k) = m;
108 |         PP(:,:,k) = P;
109 |     end  
110 |   
111 |     subplot(2,1,1);
112 |     h = plot(T,Y,'k.',T,X(1,:),'r-',T,MM(1,:),'b--');
113 |     set(h,'Linewidth',5);
114 |     title('GHKF estimate');
115 |     legend('Measurements','True','Estimate');
116 | 
117 |     subplot(2,1,2);
118 |     h = plot(T,squeeze(PP(1,1,:)),'b--');
119 | 
120 |     rmse_ghkf = sqrt(mean((X(1,:)-MM(1,:)).^2))
121 | 
122 |     gh_MM = MM;
123 |     
124 | %%
125 | % Smoother
126 | %
127 | 
128 |     ms = m;
129 |     Ps = P;
130 |     MS = zeros(size(m,1),length(Y));
131 |     PS = zeros(size(P,1),size(P,2),length(Y));
132 |     MMS = zeros(size(m,1),length(Y));
133 |     PPS = zeros(size(P,1),size(P,2),length(Y));
134 |     MMS(:,k) = m;
135 |     PPS(:,:,k) = P;
136 |     for k=size(MM,2)-1:-1:1
137 |         m = MM(:,k);
138 |         P = PP(:,:,k);
139 |     
140 |         % Form the sigma points for dynamic model
141 |         SX = repmat(m,1,size(XI,2)) + chol(P,'lower') * XI;
142 | 
143 |         % Propagate through the dynamic model
144 |         HX = [SX(1,:)+SX(2,:)*DT; SX(2,:)-g*sin(SX(1,:))*DT];
145 |   
146 |         % Compute the predicted mean and covariance
147 |         % and the cross-covariance D.
148 |         mp = zeros(size(m));
149 |         Pp = zeros(size(P));
150 |         D  = zeros(size(P));
151 |         for i=1:size(HX,2)
152 |             mp = mp + W(i) * HX(:,i);
153 |         end
154 |         for i=1:size(HX,2)
155 |             Pp = Pp + W(i) * (HX(:,i) - mp) * (HX(:,i) - mp)';
156 |             D = D + W(i) * (SX(:,i) - m) * (HX(:,i) - mp)';
157 |         end
158 |         Pp = Pp + Q;
159 | 
160 |         Gk = D/Pp;
161 |         ms = m + Gk*(ms - mp);
162 |         Ps = P + Gk*(Ps - Pp)*Gk';
163 |         MMS(:,k) = ms;
164 |         PPS(:,:,k) = Ps;
165 |     end
166 |   
167 |     h = plot(T,Y,'k.',T,X(1,:),'r-',...
168 |              T,MM(1,:),'b--',T,MMS(1,:),'g--');
169 |     set(h,'Linewidth',5);
170 |     title('GHKF and GHRTS estimates');
171 |     legend('Measurements','True','GHKF','GHRTS');
172 |     
173 |     rmse_ghrts = sqrt(mean((X(1,:)-MMS(1,:)).^2))
174 |     
175 |     gh_MMS = MMS;
176 |         
177 | %%
178 | % Plot the filtering result
179 | %
180 | 
181 |     clf;
182 |     h=plot(T,X(1,:),'k',T,Y,'bo',T,MM(1,:),'r');
183 |   
184 |     legend('True angle','Measurements','GHKF estimate');
185 |     xlabel('Time{\it t}');
186 |     ylabel('Pendulum angle {\it{x}}_{1,{\it{k}}}') 
187 |     
188 | 
189 | %%
190 | % Plot the smoothing result
191 | %
192 | 
193 |     clf;
194 |     h=plot(T,X(1,:),'k',T,Y,'bo',T,MMS(1,:),'r');
195 |   
196 |     legend('True angle','Measurements','GHRTSS estimate');
197 |     xlabel('Time{\it t}');
198 |     ylabel('Pendulum angle {\it{x}}_{1,{\it{k}}}') 
199 |    
200 | 


--------------------------------------------------------------------------------
/matlab/examples/pendulum_iekf.m:
--------------------------------------------------------------------------------
  1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | %
  3 | % Estimate pendulum state with IEKF and IERTS as in Examples 7.10 and 13.6 of
  4 | % the book
  5 | %
  6 | % Simo Sarkka and Lennart Svensson (2023), Bayesian Filtering and Smoothing,
  7 | % 2nd ed., Cambridge University Press.
  8 | % 
  9 | % See LICENSE provided with the software.
 10 | %
 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 12 |     
 13 | %%
 14 | % Simulate data
 15 | %
 16 |     pendulum_sim;
 17 | 
 18 | %%
 19 | % Filter
 20 | %
 21 |     iter = 10;
 22 | 
 23 |     m = m0;
 24 |     P = P0;
 25 |     MM = zeros(size(m,1),length(Y));
 26 |     PP = zeros(size(P,1),size(P,2),length(Y));
 27 |     for k=1:length(Y)
 28 |         f = [m(1)+m(2)*DT; m(2)-g*sin(m(1))*DT];
 29 |         F = [1 DT; -g*cos(m(1))*DT 1];
 30 |         m = f;
 31 |         P = F*P*F' + Q;
 32 |     
 33 |         mp = m;
 34 | 
 35 |         for i=1:iter
 36 |             h = sin(m(1));
 37 |             H = [cos(m(1)) 0];
 38 |             v = Y(k) - h - H * (mp - m);
 39 |             S = H*P*H' + R;
 40 |             K = P*H'/S;
 41 |             m = mp + K*v;
 42 |         end
 43 |         P = P - K*S*K';
 44 |     
 45 |         MM(:,k) = m;
 46 |         PP(:,:,k) = P;
 47 |     end
 48 |   
 49 |     subplot(2,1,1);
 50 |     h = plot(T,Y,'k.',T,X(1,:),'r-',T,MM(1,:),'b--');
 51 |     set(h,'Linewidth',5);
 52 |     title('IEKF estimate');
 53 |     legend('Measurements','True','Estimate');
 54 | 
 55 |     subplot(2,1,2);
 56 |     h = plot(T,squeeze(PP(1,1,:)),'b--');
 57 |     
 58 |     rmse_iekf = sqrt(mean((X(1,:)-MM(1,:)).^2))
 59 |   
 60 | %%
 61 | % Smoother
 62 | %
 63 | 
 64 |     iter = 10;
 65 | 
 66 |     Nm0  = m0;
 67 |     NP0  = P0;
 68 |     NMMS = MM; % Initialize with IEKF (could do with basic EKF)
 69 |     NPPS = PP;
 70 | 
 71 |     for i=1:iter
 72 |         m = m0;
 73 |         P = P0;
 74 |         for k=1:length(Y)
 75 |             if k > 1
 76 |                 nm = NMMS(:,k-1);
 77 |             else
 78 |                 nm = Nm0;
 79 |             end
 80 | 
 81 |             F = [1 DT; -g*cos(nm(1))*DT 1];
 82 |             f = [nm(1)+nm(2)*DT; nm(2)-g*sin(nm(1))*DT] + F*(m - nm);
 83 |             m = f;
 84 |             P = F*P*F' + Q;
 85 |         
 86 |             nm = NMMS(:,k);
 87 | 
 88 |             H = [cos(nm(1)) 0];
 89 |             h = sin(nm(1)) + H*(m - nm);
 90 | 
 91 |             S = H*P*H' + R;
 92 |             K = P*H'/S;
 93 |             m = m + K*(Y(k) - h);
 94 |             P = P - K*S*K';
 95 |         
 96 |             MM(:,k) = m;
 97 |             PP(:,:,k) = P;
 98 |         end
 99 |         
100 |         ms = m;
101 |         Ps = P;
102 |         MMS = MM;
103 |         PPS = PP;
104 |         for k=size(MM,2)-1:-1:0
105 |             if k == 0
106 |                 m = m0;
107 |                 P = P0;
108 |                 nm = Nm0;
109 |                 nP = NP0;
110 |             else
111 |                 m = MM(:,k);
112 |                 P = PP(:,:,k);
113 |                 nm = NMMS(:,k);
114 |                 nP = NPPS(:,:,k);
115 |             end
116 |                     
117 |             F = [1 DT; -g*cos(nm(1))*DT 1];
118 |             f = [nm(1)+nm(2)*DT; nm(2)-g*sin(nm(1))*DT] + F*(m - nm);
119 | 
120 |             mp = f;
121 |             Pp = F*P*F'+Q;
122 |             Ck = P*F'/Pp;
123 | 
124 |             ms = m + Ck*(ms - mp);
125 |             Ps = P + Ck*(Ps - Pp)*Ck';
126 |             Ps = 0.5 * (Ps + Ps');
127 | 
128 |             if k == 0
129 |                 Nm0 = ms;
130 |                 NP0 = Ps;
131 |             else
132 |                 MMS(:,k) = ms;
133 |                 PPS(:,:,k) = Ps;
134 |             end
135 |         end
136 |         
137 |         NMMS = MMS;
138 |         NPPS = PPS;
139 | 
140 |         rmse_ieks = sqrt(mean((X(1,:)-MMS(1,:)).^2));
141 |         fprintf('IEKS RMSE %d/%d = %f\n',i,iter,rmse_ieks);
142 |     end
143 | 
144 |     h = plot(T,Y,'k.',T,X(1,:),'r-',...
145 |              T,MM(1,:),'b--',T,MMS(1,:),'g--');
146 |     set(h,'Linewidth',5);
147 |     title('IEKF and IEKS estimates');
148 |     legend('Measurements','True','IEKF','IEKS');
149 |     
150 |     rmse_ieks = sqrt(mean((X(1,:)-MMS(1,:)).^2))
151 | 
152 | %%
153 | % Plot the filtering result
154 | %
155 | 
156 |     clf;
157 |     h=plot(T,X(1,:),'k',T,Y,'bo',T,MM(1,:),'r');
158 |   
159 |     legend('True angle','Measurements','IEKF estimate');
160 |     xlabel('Time{\it t}');
161 |     ylabel('Pendulum angle {\it{x}}_{1,{\it{k}}}') 
162 |     
163 | %%
164 | % Plot the smoothing result
165 | %
166 | 
167 |     clf;
168 |     h=plot(T,X(1,:),'k',T,Y,'bo',T,MMS(1,:),'r');
169 |   
170 |     legend('True angle','Measurements','IERTSS estimate');
171 |     xlabel('Time{\it t}');
172 |     ylabel('Pendulum angle {\it{x}}_{1,{\it{k}}}') 
173 |    
174 | 


--------------------------------------------------------------------------------
/matlab/examples/pendulum_iplf.m:
--------------------------------------------------------------------------------
  1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | %
  3 | % Estimate pendulum state with IEKF and IERTS as in Examples 10.13 and 14.23
  4 | % of the book
  5 | %
  6 | % Simo Sarkka and Lennart Svensson (2023), Bayesian Filtering and Smoothing,
  7 | % 2nd ed., Cambridge University Press.
  8 | % 
  9 | % See LICENSE provided with the software.
 10 | %
 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 12 |     
 13 | %%
 14 | % Simulate data
 15 | %
 16 |     pendulum_sim;
 17 | 
 18 | %%
 19 | % Select the sigma points
 20 | %
 21 |     gf_type = 1;
 22 | 
 23 |     if gf_type == 1
 24 |         name = 'ghkf'
 25 |         p = 3;
 26 |         [W,XI] = gh_ws(size(m0,1),p);
 27 |     elseif gf_type == 2
 28 |         name = 'ckf'
 29 |         [W,XI] = ckf_ws(size(m0,1));
 30 |     elseif gf_type == 3
 31 |         name = 'ukf'
 32 |         [W,XI] = sut_ws(size(m0,1));
 33 |     elseif gf_type == 4
 34 |         name = 'ukf5'
 35 |         [W,XI] = ut5_ws(size(m0,1));
 36 |     end
 37 | 
 38 | 
 39 | %%
 40 | % Filter
 41 | %
 42 | 
 43 |     iter = 10;
 44 | 
 45 |     m = m0;
 46 |     P = P0;
 47 |     MM = zeros(size(m,1),length(Y));
 48 |     PP = zeros(size(P,1),size(P,2),length(Y));
 49 |     for k=1:length(Y)
 50 |         % Form the sigma points for dynamic model
 51 |         SX = repmat(m,1,size(XI,2)) + chol(P,'lower') * XI;
 52 | 
 53 |         % Propagate through the dynamic model
 54 |         HX = [SX(1,:)+SX(2,:)*DT; SX(2,:)-g*sin(SX(1,:))*DT];
 55 |   
 56 |         % Compute the predicted mean and covariance
 57 |         m = zeros(size(m));
 58 |         P = zeros(size(P));
 59 |         for i=1:size(HX,2)
 60 |             m = m + W(i) * HX(:,i);
 61 |         end
 62 |         for i=1:size(HX,2)
 63 |             P = P + W(i) * (HX(:,i) - m) * (HX(:,i) - m)';
 64 |         end
 65 |         P = P + Q;
 66 | 
 67 |         mp = m;
 68 |         Pp = P;
 69 | 
 70 |         for it=1:iter
 71 |             % Compute the required statistics
 72 |             SX = repmat(m,1,size(XI,2)) + chol(P,'lower') * XI;
 73 |             HY = sin(SX(1,:)); 
 74 | 
 75 |             mup = zeros(size(HY,1),1);
 76 |             Py  = zeros(size(HY,1),size(HY,1));
 77 |             Pxy = zeros(size(SX,1),size(HY,1));
 78 |             for i=1:size(SX,2)
 79 |                 mup = mup + W(i) * HY(:,i);
 80 |             end
 81 |             for i=1:size(SX,2)
 82 |                 Py = Py + W(i) * (HY(:,i) - mup) * (HY(:,i) - mup)';
 83 |                 Pxy = Pxy + W(i) * (SX(:,i) - m) * (HY(:,i) - mup)';
 84 |             end
 85 |             Py = Py + R;
 86 | 
 87 |             H = Pxy' / P;
 88 |             b = mup -  H * m;
 89 |             O = Py - H * P * H';
 90 | 
 91 |             S = H * Pp * H' + O;
 92 |             K = Pp * H' / S;
 93 |             m = mp + K * (Y(:,k) - H * mp - b);
 94 |             P = Pp - K * S * K';
 95 |             P = 0.5 * (P + P');
 96 |         end
 97 |         MM(:,k) = m;
 98 |         PP(:,:,k) = P;
 99 |     end
100 |   
101 |     subplot(2,1,1);
102 |     h = plot(T,Y,'k.',T,X(1,:),'r-',T,MM(1,:),'b--');
103 |     set(h,'Linewidth',5);
104 |     title('IPLF estimate');
105 |     legend('Measurements','True','Estimate');
106 | 
107 |     subplot(2,1,2);
108 |     h = plot(T,squeeze(PP(1,1,:)),'b--');
109 |     
110 |     rmse_iplf = sqrt(mean((X(1,:)-MM(1,:)).^2))
111 |   
112 | %%
113 | % Smoother
114 | %
115 | 
116 |     iter = 10;
117 | 
118 |     Nm0  = m0;
119 |     NP0  = P0;
120 |     NMMS = MM; % Initialize with IPLF
121 |     NPPS = PP;
122 | 
123 |     nx = size(m0,1);
124 |     ny = size(Y,1);
125 | 
126 |     As = zeros(nx,nx,size(Y,2));
127 |     as = zeros(nx,size(Y,2));
128 |     Ls = zeros(nx,nx,size(Y,2));
129 |     
130 |     Hs = zeros(ny,nx,size(Y,2));
131 |     bs = zeros(ny,size(Y,2));
132 |     Os = zeros(ny,ny,size(Y,2));
133 | 
134 |     MMi = zeros(size(MM));
135 |     PPi = zeros(size(PP));
136 |     MMSi = zeros(size(MM));
137 |     PPSi = zeros(size(PP));
138 | 
139 |     for it=1:iter
140 | 
141 |         % Linearize the whole thing
142 |         for k=1:length(Y)
143 |             if k > 1
144 |                 m = NMMS(:,k-1);
145 |                 P = NPPS(:,:,k-1);
146 |             else
147 |                 m = Nm0;
148 |                 P = NP0;
149 |             end
150 | 
151 |             SX = repmat(m,1,size(XI,2)) + chol(P,'lower') * XI;
152 |             HX = [SX(1,:)+SX(2,:)*DT; SX(2,:)-g*sin(SX(1,:))*DT];
153 |       
154 |             mum = zeros(size(m));
155 |             for i=1:size(HX,2)
156 |                 mum = mum + W(i) * HX(:,i);
157 |             end
158 |             Px  = zeros(size(P));
159 |             Pxx = zeros(size(P));
160 |             for i=1:size(HX,2)
161 |                 Px = Px + W(i) * (HX(:,i) - mum) * (HX(:,i) - mum)';
162 |                 Pxx = Pxx + W(i) * (SX(:,i) - m) * (HX(:,i) - mum)';
163 |             end
164 |             Px = Px + Q;
165 |             
166 |             A = Pxx' / P;
167 |             a = mum - A * m;
168 |             L = Px - A * P * A';
169 | 
170 | 
171 |             % Note that we store A_k to As(:,:,k+1) etc.
172 |             As(:,:,k) = A;
173 |             as(:,k) = a;
174 |             Ls(:,:,k) = L;
175 |         end
176 | 
177 |         for k=1:length(Y)
178 |             m = NMMS(:,k);
179 |             P = NPPS(:,:,k);
180 | 
181 |             SX = repmat(m,1,size(XI,2)) + chol(P,'lower') * XI;
182 |             HY = sin(SX(1,:)); 
183 | 
184 |             mup = zeros(size(HY,1),1);
185 |             Py  = zeros(size(HY,1),size(HY,1));
186 |             Pxy = zeros(size(SX,1),size(HY,1));
187 |             for i=1:size(SX,2)
188 |                 mup = mup + W(i) * HY(:,i);
189 |             end
190 |             for i=1:size(SX,2)
191 |                 Py = Py + W(i) * (HY(:,i) - mup) * (HY(:,i) - mup)';
192 |                 Pxy = Pxy + W(i) * (SX(:,i) - m) * (HY(:,i) - mup)';
193 |             end
194 |             Py = Py + R;
195 | 
196 |             H = Pxy' / P;
197 |             b = mup - H * m;
198 |             O = Py - H * P * H';
199 | 
200 |             Hs(:,:,k) = H;
201 |             bs(:,k) = b;
202 |             Os(:,:,k) = O;
203 |         end
204 | 
205 |         % Run affine filter and smoother
206 |         m = m0;
207 |         P = P0;
208 |         for k=1:length(Y)
209 |             A = As(:,:,k);
210 |             a = as(:,k);
211 |             L = Ls(:,:,k);
212 |             
213 |             m = A * m + a;
214 |             P = A * P * A' + L;
215 |         
216 |             H = Hs(:,:,k);
217 |             b = bs(:,k);
218 |             O = Os(:,:,k);
219 | 
220 |             mu = H * m + b;
221 |             S = H * P * H' + O;
222 |             K = P * H' / S;
223 |             m = m + K * (Y(:,k) - mu);
224 |             P = P - K * S * K';
225 |         
226 |             MMi(:,k) = m;
227 |             PPi(:,:,k) = P;
228 |         end
229 |         
230 |         ms = m;
231 |         Ps = P;
232 |         MMSi(:,end) = m;
233 |         PPSi(:,:,end) = P;
234 |         for k=size(MMi,2)-1:-1:0
235 |             if k == 0
236 |                 m = m0;
237 |                 P = P0;
238 |             else
239 |                 m = MMi(:,k);
240 |                 P = PPi(:,:,k);
241 |             end
242 | 
243 |             A = As(:,:,k+1);
244 |             a = as(:,k+1);
245 |             L = Ls(:,:,k+1);
246 | 
247 |             mp = A * m + a;
248 |             Pp = A * P * A' + L;
249 |             G  = P * A' / Pp;
250 | 
251 |             ms = m + G * (ms - mp);
252 |             Ps = P + G * (Ps - Pp) * G';
253 |             Ps = 0.5 * (Ps + Ps');
254 | 
255 |             if k == 0
256 |                 Nm0 = ms;
257 |                 NP0 = Ps;
258 |             else
259 |                 MMSi(:,k) = ms;
260 |                 PPSi(:,:,k) = Ps;
261 |             end
262 |         end
263 |         
264 |         NMMS = MMSi;
265 |         NPPS = PPSi;
266 | 
267 |         MMS = MMSi;
268 |         PPPS = PPSi;
269 | 
270 |         rmse_ipls = sqrt(mean((X(1,:)-MMSi(1,:)).^2));
271 |         fprintf('IPLS RMSE %d/%d = %f\n',it,iter,rmse_ipls);
272 |     end
273 | 
274 |     h = plot(T,Y,'k.',T,X(1,:),'r-',...
275 |              T,MMi(1,:),'b--',T,MMSi(1,:),'g--');
276 |     set(h,'Linewidth',5);
277 |     title('IPLF and IPLS estimates');
278 |     legend('Measurements','True','IPLF','IPLS');
279 |     
280 |     rmse_ipls = sqrt(mean((X(1,:)-MMSi(1,:)).^2))
281 | 
282 | %%
283 | % Plot the filtering result
284 | %
285 | 
286 |     clf;
287 |     h=plot(T,X(1,:),'k',T,Y,'bo',T,MM(1,:),'r');
288 |   
289 |     legend('True angle','Measurements','IPLF estimate');
290 |     xlabel('Time{\it t}');
291 |     ylabel('Pendulum angle {\it{x}}_{1,{\it{k}}}') 
292 |     
293 | 
294 | %%
295 | % Plot the smoothing result
296 | %
297 | 
298 |     clf;
299 |     h=plot(T,X(1,:),'k',T,Y,'bo',T,MMS(1,:),'r');
300 |   
301 |     legend('True angle','Measurements','IPLS estimate');
302 |     xlabel('Time{\it t}');
303 |     ylabel('Pendulum angle {\it{x}}_{1,{\it{k}}}') 
304 | 


--------------------------------------------------------------------------------
/matlab/examples/pendulum_par.m:
--------------------------------------------------------------------------------
  1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | %
  3 | % Pendulum parameter posterior estimation with GHKF and PMCMC
  4 | % as in Example 16.17 of the book
  5 | %
  6 | % Simo Sarkka and Lennart Svensson (2023), Bayesian Filtering and Smoothing,
  7 | % 2nd ed., Cambridge University Press.
  8 | % 
  9 | % See LICENSE provided with the software.
 10 | %
 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 12 | 
 13 | %%
 14 | % Simulate data
 15 | %
 16 |     pendulum_sim;
 17 | 
 18 | %%
 19 | % Estimate pendulum the posterior with GHKF
 20 | %
 21 | 
 22 |     p = 5; % Order of the method
 23 |     n = 2;
 24 | 
 25 |     %
 26 |     % Form Probabilists' Hermite polynomials of
 27 |     % order p-1 and p
 28 |     %
 29 |     Hpm = 1;
 30 |     Hp  = [1 0];
 31 |     for i=1:p-1
 32 |         tmp = Hp;
 33 |         Hp = [Hp 0] - [0 0 i*Hpm];
 34 |         Hpm = tmp;
 35 |     end
 36 | 
 37 |     %
 38 |     % Single dimensional weights and points
 39 |     %
 40 |     xi1 = roots(Hp)';
 41 |     W1 = factorial(p)./(p^2*polyval(Hpm,xi1).^2);
 42 |      
 43 |     %Generate all p^n collections of indexes by
 44 |     %transforming numbers 0...p^n-1) into p-base system
 45 |     %and by adding 1 to each digit
 46 |     num = 0:(p^n-1);
 47 |     ind = zeros(n,p^n);
 48 |     for i=1:n
 49 |         ind(i,:) = rem(num,p)+1;
 50 |         num = floor(num / p);
 51 |     end
 52 | 
 53 |     %Form the unit sigma points and weights
 54 |     XI = xi1(ind);
 55 |     W = prod(W1(ind),1); % ND weights
 56 | 
 57 |     
 58 |     f = @(x) [x(1)+x(2)*DT; x(2)-g*sin(x(1))*DT];
 59 |     h = @(x) sin(x(1));
 60 | 
 61 |     dR = 0.001;
 62 |     %dR = 0.01; % A bit less dense grid
 63 |     RR = 0.05:dR:0.15;
 64 |     pp = zeros(size(RR));
 65 |     for ri=1:length(RR)
 66 |         m = m0;
 67 |         P = P0;
 68 |         
 69 |         energy = 0;
 70 |         
 71 |         MM = zeros(size(m,1),size(Y,2));
 72 |         PP = zeros(size(P,1),size(P,2),size(Y,2));
 73 |         for k=1:size(Y,2)
 74 |             SX = repmat(m,1,size(XI,2)) + chol(P,'lower') * XI;
 75 |             m = zeros(size(m));
 76 |             for i=1:size(SX,2)
 77 |                 SX(:,i) = f(SX(:,i));
 78 |                 m = m + W(i) * SX(:,i);
 79 |             end
 80 |             P = Q;
 81 |             for i=1:size(SX,2)
 82 |                 P = P + W(i) * (SX(:,i) - m) * (SX(:,i) - m)';
 83 |             end
 84 |             
 85 |             SX = repmat(m,1,size(XI,2)) + chol(P,'lower') * XI;
 86 |             SY = zeros(size(Y,1),size(SX,2));
 87 |             mu = zeros(size(Y,1),1);
 88 |             for i=1:size(SX,2)
 89 |                 SY(:,i) = h(SX(:,i));
 90 |                 mu = mu + W(i) * SY(:,i);
 91 |             end
 92 |             S = RR(ri);
 93 |             C = zeros(size(SX,1),size(SY,1));
 94 |             for i=1:size(SY,2)
 95 |                 S = S + W(i) * (SY(:,i) - mu) * (SY(:,i) - mu)';
 96 |                 C = C + W(i) * (SX(:,i) - m) * (SY(:,i) - mu)';
 97 |             end
 98 |             
 99 |             v = Y(:,k) - mu;
100 |             K = C / S;
101 |             m = m + K * v;
102 |             P = P - K * S * K';
103 |             
104 |             energy = energy + 0.5 * log(det(2*pi*S)) + 0.5 * v'*(S\v);
105 |             
106 |             MM(:,k) = m;
107 |             PP(:,:,k) = P;
108 |         end
109 | 
110 |         pp(ri) = energy;
111 |     end
112 |     
113 |     pp = pp - min(pp);
114 |     pp = exp(-pp);
115 |     pp = pp ./ sum(pp) / dR;
116 |     plot(RR,pp);
117 |     
118 | %%
119 | % PMCMC
120 | %
121 | 
122 |     % You can comment this out
123 |     rng(1, 'twister');
124 | 
125 |     N = 1000; % PF samples
126 |     %N = 100; % PF samples
127 |     nmc = 10000; % MCMC samples
128 |     %nmc = 1000; % A bit less
129 | 
130 |     QL = chol(Q,'lower');
131 | 
132 |     
133 |     doram = 1;
134 |     samp = [];
135 |     as = 0.234;
136 |     S  = 0.25;
137 |     
138 |     theta = -2;
139 | 
140 |     en = 0;
141 |     accepted = 0;
142 |     
143 |     for j=1:nmc
144 | 
145 |         %
146 |         % Draw candidate
147 |         %
148 |         r = randn;
149 |         new_theta = theta + S * r;
150 | 
151 |         %
152 |         % Evalaute energy with PF
153 |         %        
154 |         m = m0;
155 |         P = P0;
156 |         
157 |         %
158 |         % Initial sample set
159 |         %
160 |         SX = repmat(m,1,N) + chol(P,'lower') * randn(size(m,1), N);
161 |   
162 |         %
163 |         % Do the filtering and store the histories
164 |         %  
165 |         MM  = zeros(size(m,1),length(Y));
166 |         SSX = zeros(size(m,1),N,length(Y)); % filter history
167 |         
168 |         new_en = 0;
169 |         for k=1:length(Y)
170 | 
171 |             % Propagate through the dynamic model
172 |             SX = [SX(1,:)+SX(2,:)*DT; SX(2,:)-g*sin(SX(1,:))*DT];
173 |     
174 |             % Add the process noise
175 |             SX = SX + QL * randn(size(SX));
176 |     
177 |             % Compute the weights
178 |             my = sin(SX(1,:));
179 |             W  = 1/sqrt(2*pi*exp(new_theta)) * exp(-1/(2*exp(new_theta))*(Y(k) - my).^2);
180 |             lh = mean(W);
181 |             new_en = new_en - log(lh);
182 |             W  = W ./ sum(W);
183 |     
184 |             % Do resampling
185 |             ind = resampstr(W);
186 |             SX  = SX(:,ind);      
187 |     
188 |             %if rem(k,100)==0
189 |             %    fprintf('%d/%d\n',k,length(Y));
190 |             %end
191 |         end
192 |         
193 |         %
194 |         % Accept or reject
195 |         %
196 |         if j == 1
197 |             en = new_en;
198 |         end
199 |         a = min(1,exp(en - new_en));
200 |         u = rand;
201 | 
202 |         if u <= a
203 |             theta = new_theta;
204 |             en    = new_en;
205 |             accepted = accepted + 1;
206 | %            fprintf('Accepted: %f\n',theta);
207 |         else
208 | %            fprintf('Rejected\n');
209 |         end
210 |         samp = [samp theta];
211 | 
212 |         %
213 |         % Adapt
214 |         %
215 |         if doram && j > 10
216 |             nu = j^(-0.9);
217 |             S = sqrt(S * (1 + nu * (a - as) * r*r/r^2) * S);
218 |         end
219 |         
220 |         if rem(j,10) == 0
221 |             fprintf('%d/%d Acceptance rate %f\n',j,nmc,accepted/j);        
222 |             subplot(2,1,1);
223 |             plot(samp);
224 |             subplot(2,1,2);
225 |             hist(exp(samp),10)
226 |             drawnow;
227 |         end
228 |     end
229 |     
230 | %%
231 | % Plotting
232 | %
233 | 
234 |     [HN,HX] = hist(exp(samp(100:end)),100);
235 |     HN = HN / sum(HN) / (HX(2)-HX(1));
236 |     HN = [0 HN 0];
237 |     HX = [HX(1) HX HX(end)];
238 | 
239 |     clf;
240 |     h = fill(HX,HN,'g');
241 |     set(h,'FaceColor',0.5 * [1 1 1]);
242 |     set(h,'EdgeColor',0.5 * [1 1 1]);
243 |     hold on;
244 |     h = plot(RR,pp,'-');
245 |     set(h,'Color',0.0 * [1 1 1]);
246 |     set(h,'LineWidth',1)
247 |     ax = axis;
248 |     h = plot([R R],[0 ax(end)],'-');
249 |     set(h,'Color',0.7 * [1 1 1]);
250 |     set(h,'LineWidth',2)
251 | 
252 |     xlabel('{\it R}');
253 |     ylabel('{\it p}({\it{R}} |{\it y_{{\rm{1}}:T}} )')
254 | 
255 |     legend('PMCMC histogram','Gaussian filter estimate',...
256 |            'True parameter value');
257 | 


--------------------------------------------------------------------------------
/matlab/examples/pendulum_pf.m:
--------------------------------------------------------------------------------
 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 2 | %
 3 | % Estimate pendulum state with a particle filter as in Example 11.10 of
 4 | % the book
 5 | %
 6 | % Simo Sarkka and Lennart Svensson (2023), Bayesian Filtering and Smoothing,
 7 | % 2nd ed., Cambridge University Press.
 8 | % 
 9 | % See LICENSE provided with the software.
10 | %
11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
12 | 
13 |     %%
14 |     % Simulate data
15 |     %
16 |     pendulum_sim;
17 | 
18 |     %%
19 |     % Filter
20 |     %
21 | 
22 |     % You can comment this out
23 |     rng(1, 'twister');
24 | 
25 |     m = m0;
26 |     P = P0;
27 |     N = 10000;
28 | 
29 |     QL = chol(Q,'lower');
30 |     
31 |     %
32 |     % Initial sample set
33 |     %
34 |     SX = repmat(m,1,N) + chol(P,'lower') * randn(size(m,1), N);
35 | 
36 |     %
37 |     % Do the filtering and store the histories
38 |     %  
39 |     MM  = zeros(size(m,1),length(Y));
40 |     SSX = zeros(size(m,1),N,length(Y)); % filter history
41 |     for k=1:length(Y)
42 | 
43 |         % Propagate through the dynamic model
44 |         SX = [SX(1,:)+SX(2,:)*DT; SX(2,:)-g*sin(SX(1,:))*DT];
45 |     
46 |         % Add the process noise
47 |         SX = SX + QL * randn(size(SX));
48 |     
49 |         % Compute the weights
50 |         my = sin(SX(1,:));
51 |         W  = exp(-1/(2*R)*(Y(k) - my).^2); % Constant discarded
52 |         W  = W ./ sum(W);
53 |     
54 |         % Do resampling
55 |         ind = resampling(W,'stratified');
56 |         SX   = SX(:,ind);      
57 |     
58 |         SSX(:,:,k) = SX;
59 |         % Mean estimate
60 |         m = mean(SX,2);
61 |     
62 |         MM(:,k) = m;
63 |         if rem(k,100)==0
64 |             fprintf('%d/%d\n',k,length(Y));
65 |         end
66 |     end
67 | 
68 |     h = plot(T,Y,'k.',T,X(1,:),'r-',T,MM(1,:),'b--');
69 |     set(h,'Linewidth',5);
70 |     fprintf('BF estimate.\n');
71 |     legend('Measurements','True','Estimate');
72 |   
73 |     rmse_bf = sqrt(mean((X(1,:)-MM(1,:)).^2))
74 | 
75 |   
76 |     %%
77 |     % Plot the final filtering figure
78 |     %
79 |     clf;
80 |     h=plot(T,X(1,:),'k',T,Y,'bo',T,MM(1,:),'r');
81 |   
82 |     xlabel('Time{\it t}');
83 |     ylabel('Pendulum angle {\it{x}}_{1,{\it{k}}}') 
84 |     legend('True angle','Measurements','PF estimate');
85 | 


--------------------------------------------------------------------------------
/matlab/examples/pendulum_pf2.m:
--------------------------------------------------------------------------------
  1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | %
  3 | % Estimate cluttered pendulum state with a particle filter as in Examples
  4 | % 11.11 and 15.5 of the book
  5 | %
  6 | % Simo Sarkka and Lennart Svensson (2023), Bayesian Filtering and Smoothing,
  7 | % 2nd ed., Cambridge University Press.
  8 | % 
  9 | % See LICENSE provided with the software.
 10 | %
 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 12 | 
 13 |     %%
 14 |     % Simulate data
 15 |     %
 16 |     pendulum_sim2;
 17 | 
 18 |     %%
 19 |     % GHKF
 20 |     %
 21 |     m = m0;
 22 |     P = P0;
 23 |     
 24 |     n = size(m,1);
 25 | 
 26 |     p = 5;
 27 |     [W, XI] = gh_ws(n,p);
 28 |     
 29 |     %
 30 |     % Do the filtering
 31 |     %  
 32 |     MM = zeros(size(m,1),length(Y));
 33 |     PP = zeros(size(P,1),size(P,2),length(Y));
 34 |     for k=1:length(Y)
 35 | 
 36 |         % Form the sigma points for dynamic model
 37 |         SX = repmat(m,1,size(XI,2)) + chol(P,'lower') * XI;
 38 | 
 39 |         % Propagate through the dynamic model
 40 |         HX = [SX(1,:)+SX(2,:)*DT; SX(2,:)-g*sin(SX(1,:))*DT];
 41 |   
 42 |         % Compute the predicted mean and covariance
 43 |         m = zeros(size(m));
 44 |         P = zeros(size(P));
 45 |         for i=1:size(HX,2)
 46 |             m = m + W(i) * HX(:,i);
 47 |         end
 48 |         for i=1:size(HX,2)
 49 |             P = P + W(i) * (HX(:,i) - m) * (HX(:,i) - m)';
 50 |         end
 51 |         P = P + Q;
 52 | 
 53 |         % Form sigma points for measurement step and
 54 |         % propagate throught the measurement model
 55 |         SX = repmat(m,1,size(XI,2)) + chol(P,'lower') * XI;
 56 |         HY = sin(SX(1,:)); 
 57 |     
 58 |         % Compute the updated mean and covariance
 59 |         mu = zeros(size(HY,1),1);
 60 |         S  = zeros(size(HY,1),size(HY,1));
 61 |         C  = zeros(size(SX,1),size(HY,1));
 62 |         for i=1:size(SX,2)
 63 |             mu = mu + W(i) * HY(:,i);
 64 |         end
 65 |         for i=1:size(SX,2)
 66 |             S = S + W(i) * (HY(:,i) - mu) * (HY(:,i) - mu)';
 67 |             C = C + W(i) * (SX(:,i) - m) * (HY(:,i) - mu)';
 68 |         end
 69 |         S = S + R;
 70 |     
 71 |         % Compute the gain and updated mean and covariance  
 72 |         K = C/S;
 73 |         m = m + K*(Y(k) - mu);
 74 |         P = P - K*S*K';
 75 |     
 76 |         MM(:,k) = m;
 77 |         PP(:,:,k) = P;
 78 |     end  
 79 |     ghkf_MM = MM;
 80 |     ghkf_PP = PP;
 81 | 
 82 |     clf;
 83 |     h = plot(T,Y,'k.',T,X(1,:),'r-',T,MM(1,:),'b--');
 84 |     set(h,'Linewidth',5);
 85 |     title('GHKF estimate');
 86 |     legend('Measurements','True','Estimate');
 87 | 
 88 |     rmse_ghkf = sqrt(mean((X(1,:)-MM(1,:)).^2))
 89 | 
 90 |     %%
 91 |     % GHRTS
 92 |     %
 93 |     ms = m;
 94 |     Ps = P;
 95 |     MS = zeros(size(m,1),length(Y));
 96 |     PS = zeros(size(P,1),size(P,2),length(Y));
 97 |     MMS(:,k) = m;
 98 |     PPS(:,:,k) = P;
 99 |     for k=size(MM,2)-1:-1:1
100 |         m = MM(:,k);
101 |         P = PP(:,:,k);
102 |     
103 |         % Form the sigma points for dynamic model
104 |         SX = repmat(m,1,size(XI,2)) + chol(P,'lower') * XI;
105 | 
106 |         % Propagate through the dynamic model
107 |         HX = [SX(1,:)+SX(2,:)*DT; SX(2,:)-g*sin(SX(1,:))*DT];
108 |  
109 |         % Compute the predicted mean and covariance
110 |         % and the cross-covariance D.
111 |         mp = zeros(size(m));
112 |         Pp = zeros(size(P));
113 |         D  = zeros(size(P));
114 |         for i=1:size(HX,2)
115 |             mp = mp + W(i) * HX(:,i);
116 |         end
117 |         for i=1:size(HX,2)
118 |             Pp = Pp + W(i) * (HX(:,i) - mp) * (HX(:,i) - mp)';
119 |             D = D + W(i) * (SX(:,i) - m) * (HX(:,i) - mp)';
120 |         end
121 |         Pp = Pp + Q;
122 | 
123 |         Gk = D/Pp;
124 |         ms = m + Gk*(ms - mp);
125 |         Ps = P + Gk*(Ps - Pp)*Gk';
126 |         MMS(:,k) = ms;
127 |         PPS(:,:,k) = Ps;
128 |     end
129 |   
130 |     ghrtss_MMS = MMS;
131 |     ghrtss_PPS = PPS;
132 | 
133 |     h = plot(T,Y,'k.',T,X(1,:),'r-',...
134 |              T,MM(1,:),'b--',T,MMS(1,:),'g--');
135 |     set(h,'Linewidth',5);
136 |     title('GHKF and GHRTS estimates');
137 |     legend('Measurements','True','GHKF','GHRTS');
138 |     
139 |     rmse_ghrts = sqrt(mean((X(1,:)-MMS(1,:)).^2))
140 | 
141 | 
142 |     %%
143 |     % Particle filter
144 |     %
145 | 
146 |     % You can comment this out
147 |     rng(1, 'twister');
148 |     
149 |     m = m0;
150 |     P = P0;
151 |     N = 10000;
152 | 
153 |     QL = chol(Q,'lower');
154 |     
155 |     %
156 |     % Initial sample set
157 |     %
158 |     SX = repmat(m,1,N) + chol(P,'lower') * randn(size(m,1), N);
159 |   
160 |     %
161 |     % Do the filtering and store the histories
162 |     %  
163 |     MM  = zeros(size(m,1),length(Y));
164 |     SSX = zeros(size(m,1),N,length(Y)); % filter history
165 |     for k=1:length(Y)
166 | 
167 |         % Propagate through the dynamic model
168 |         SX = [SX(1,:)+SX(2,:)*DT; SX(2,:)-g*sin(SX(1,:))*DT];
169 |     
170 |         % Add the process noise
171 |         SX = SX + QL * randn(size(SX));
172 |     
173 |         % Draw indicator
174 |         c = rand(1,size(SX,2)) < cp;
175 |         
176 |         % Compute the weights
177 |         ind0 = find(c == 0);
178 |         ind1 = find(c == 1);
179 |         
180 |         my = sin(SX(1,ind0));
181 |         W(ind0) = exp(-1/(2*R)*(Y(k) - my).^2); % Constant discarded
182 |         W(ind1) = 1/4;
183 |         W  = W ./ sum(W);
184 |     
185 |         % Do resampling
186 |         ind = resampling(W,'stratified');
187 |         SX   = SX(:,ind);      
188 |     
189 |         SSX(:,:,k) = SX;
190 |         % Mean estimate
191 |         m = mean(SX,2);
192 |     
193 |         MM(:,k) = m;
194 |         if rem(k,100)==0
195 |             fprintf('%d/%d\n',k,length(Y));
196 |         end
197 |     end
198 | 
199 |     pf_MM = MM;
200 | 
201 |     h = plot(T,Y,'k.',T,X(1,:),'r-',T,MM(1,:),'b--');
202 |     set(h,'Linewidth',5);
203 |     fprintf('BF estimate.\n');
204 |     legend('Measurements','True','Estimate');
205 |   
206 |     rmse_bf = sqrt(mean((X(1,:)-MM(1,:)).^2))
207 | 
208 |   
209 | %%
210 | % Backward simulation smoother
211 | %
212 | 
213 |     % You can comment this out
214 |     rng(1, 'twister');
215 | 
216 |     NS = 100;
217 |     SM_SSX = zeros(size(m,1),NS,length(Y));
218 |   
219 |     for i=1:NS
220 |         ind = floor(rand * N + 1);
221 |         xn = SSX(:,ind,end);
222 |         SM_SSX(:,i,end) = xn;
223 |         for k=length(Y)-1:-1:1
224 |             SX = SSX(:,:,k);
225 |             mu = [SX(1,:)+SX(2,:)*DT; SX(2,:)-g*sin(SX(1,:))*DT];
226 | 
227 |             % Evaluate the transition density
228 |             DX = xn-mu;
229 |             E = 0.5 * sum(DX .* (Q \ DX),1) + 0.5 * size(mu,1) * log(2*pi) + 0.5 * log(det(Q));
230 |             W = exp(-E);
231 |             W  = W ./ sum(W);
232 | 
233 |             % Draw a new sample
234 |             ind = resampling(W,'multinomial',1);
235 |             xn = SSX(:,ind,k);
236 |             SM_SSX(:,i,k) = xn;
237 |         end
238 |         if rem(i,10)==0
239 |             plot(T,X(1,:),'r-',T,squeeze(SM_SSX(1,i,:)),'b--');
240 |             title(sprintf('Doing backward simulation %d/%d\n',i,NS));
241 |             drawnow;
242 |         end
243 |     end
244 | 
245 |     MMS = squeeze(mean(SM_SSX,2));
246 |     ps_MMS = MMS;
247 |   
248 |     h = plot(T,Y,'k.',T,X(1,:),'r-',T,MMS(1,:),'b--');
249 |     set(h,'Linewidth',5);
250 |     fprintf('BS estimate.\n');
251 |     legend('Measurements','True','Estimate');
252 |   
253 |     rmse_bs = sqrt(mean((X(1,:)-MMS(1,:)).^2))
254 |   
255 | %%
256 | % Plot the filtering result
257 | %
258 | 
259 |     clf;
260 |     h=plot(T,X(1,:),'k',T,Y,'bo',T,pf_MM(1,:),'r',T,ghkf_MM(1,:),'--');
261 |   
262 |     legend('True angle','Measurements','PF estimate','GHKF estimate');
263 |     xlabel('Time{\it t}');
264 |     ylabel('Pendulum angle {\it{x}}_{1,{\it{k}}}') 
265 | 
266 | 
267 | %%
268 | % Plot the smoothing result
269 | %
270 | 
271 |     clf;
272 |     h=plot(T,X(1,:),'k',T,Y,'bo',T,ps_MMS(1,:),'r',T,ghrtss_MMS(1,:),'--');
273 |   
274 |     legend('True angle','Measurements','PS estimate','GHRTSS estimate');
275 |     xlabel('Time{\it t}');
276 |     ylabel('Pendulum angle {\it{x}}_{1,{\it{k}}}') 
277 | 


--------------------------------------------------------------------------------
/matlab/examples/pendulum_sim.m:
--------------------------------------------------------------------------------
 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 2 | %
 3 | % Simulate pendulum data for the examples in the book
 4 | %
 5 | % Simo Sarkka and Lennart Svensson (2023), Bayesian Filtering and Smoothing,
 6 | % 2nd ed., Cambridge University Press.
 7 | % 
 8 | % See LICENSE provided with the software.
 9 | %
10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
11 | 
12 |     %%
13 |     % Simulate simple pendulum. Note that the system easily
14 |     % diverges, but it should not matter.
15 |     %
16 |     
17 |     % Remember to comment these out when benchmarking!
18 |     fprintf('!! Using fixed random stream !!\n');
19 |     rng(1,'twister');
20 |     % <--- 
21 |     
22 |     DT = 0.01;
23 |     g  = 9.81;
24 |     Q  = 0.01*[DT^3/3 DT^2/2; DT^2/2 DT];
25 |     R  = 0.1;
26 | %    m0 = [1.6;0]; % Slightly off
27 | %    P0 = 0.1*eye(2);
28 |     m0 = [0;0];
29 |     P0 = eye(2);
30 |     
31 |     steps = 500;
32 |     
33 |     QL = chol(Q,'lower');
34 |     
35 |     T = zeros(1,steps);
36 |     X = zeros(2,steps);
37 |     Y = zeros(1,steps);
38 |     t = 0;
39 |     x = [1.5;0];
40 |     for k=1:steps
41 |         x = [x(1)+x(2)*DT;
42 |             x(2)-g*sin(x(1))*DT];
43 |         w = QL * randn(2,1);
44 |         x = x + w;
45 |         y = sin(x(1)) + sqrt(R)*randn;
46 |         t = t + DT;
47 |         T(k) = t;
48 |         X(:,k) = x;
49 |         Y(:,k) = y;
50 |     end
51 |     
52 |     % Plot the data
53 |     clf;
54 |     plot(T,Y,'g.',T,X(1,:),'r-');
55 | 
56 |     % Store the data in JSON
57 |     %{
58 |     jstruct = struct('T',T,'X',X,'Y',Y);
59 |     json = jsonencode(jstruct,'PrettyPrint',true);
60 |     filename = 'pendulum.json';
61 |     fid = fopen(filename, 'w');
62 |     fwrite(fid, json);
63 |     fclose(fid);
64 |     %}
65 | 
66 | 
67 | 


--------------------------------------------------------------------------------
/matlab/examples/pendulum_sim2.m:
--------------------------------------------------------------------------------
 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 2 | %
 3 | % Simulate cluttered pendulum data for the examples in the book
 4 | %
 5 | % Simo Sarkka and Lennart Svensson (2023), Bayesian Filtering and Smoothing,
 6 | % 2nd ed., Cambridge University Press.
 7 | % 
 8 | % See LICENSE provided with the software.
 9 | %
10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
11 |     
12 | % Simulate simple pendulum. Note that the system easily diverges, but
13 | % it should not matter. Also draw some clutter measurements.
14 |     
15 |     % Remember to comment these out when benchmarking!
16 |     fprintf('!! Using fixed random stream !!\n');
17 |     rng(1,'twister');
18 |     % <---
19 |     
20 |     DT = 0.01;
21 |     g  = 9.81;
22 |     Q  = 0.01*[DT^3/3 DT^2/2; DT^2/2 DT];
23 |     R  = 0.1;
24 |     m0 = [0;0];
25 |     P0 = eye(2);
26 |         
27 |     steps = 500;
28 | 
29 |     QL = chol(Q,'lower');
30 |    
31 |     T = [];
32 |     X = [];
33 |     Y = [];
34 |     t = 0;
35 |     x = [1.5;0];
36 |     for k=1:steps
37 |         x = [x(1)+x(2)*DT;
38 |              x(2)-g*sin(x(1))*DT];
39 |         w = QL * randn(2,1);
40 |         x = x + w;
41 |         y = sin(x(1)) + sqrt(R)*randn;
42 |         t = t + DT;
43 |         X = [X x];
44 |         Y = [Y y];
45 |         T = [T t];
46 |   end
47 |   
48 |   cp = 0.5;
49 |   C = rand(size(Y)) < cp;
50 |   ind = find(C);
51 |   Y(ind) = 4*rand(size(ind))-2;
52 |   
53 |   % Plot the data    
54 |   plot(T,Y,'g.',T,X(1,:),'r-');
55 | 
56 | 


--------------------------------------------------------------------------------
/matlab/examples/pendulum_ukf.m:
--------------------------------------------------------------------------------
  1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | %
  3 | % Estimate pendulum state with UKF and URTS as in Examples 8.19 and 14.10
  4 | % of the book
  5 | %
  6 | % Simo Sarkka and Lennard Svensson (2023), Bayesian Filtering and Smoothing,
  7 | % 2nd ed., Cambridge University Press.
  8 | % 
  9 | % See LICENSE provided with the software.
 10 | %
 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 12 | 
 13 | %%
 14 | % Simulate data
 15 | %
 16 |     pendulum_sim;
 17 | 
 18 | %%
 19 | % Filter
 20 | %
 21 | 
 22 |     m = m0;
 23 |     P = P0;
 24 | 
 25 |     %
 26 |     % Precompute the UT weights
 27 |     %
 28 |     n = size(m,1);
 29 |     alpha = 1;
 30 |     beta = 0;
 31 |     kappa = 3-n;
 32 | 
 33 |     lambda = alpha^2 * (n + kappa) - n;        
 34 |     WM = zeros(2*n+1,1);
 35 |     WC = zeros(2*n+1,1);
 36 |     for j=1:2*n+1
 37 |         if j==1
 38 |             wm = lambda / (n + lambda);
 39 |             wc = lambda / (n + lambda) + (1 - alpha^2 + beta);
 40 |         else
 41 |             wm = 1 / (2 * (n + lambda));
 42 |             wc = wm;
 43 |         end
 44 |         WM(j) = wm;
 45 |         WC(j) = wc;
 46 |     end
 47 | 
 48 |     %
 49 |     % Do the filtering
 50 |     %  
 51 |     MM = zeros(size(m,1),length(Y));
 52 |     PP = zeros(size(P,1),size(P,2),length(Y));
 53 |     for k=1:length(Y)
 54 | 
 55 |         % Form the sigma points for dynamic model
 56 |         A = chol(P,'lower');
 57 |         SX = [zeros(size(m)) A -A];
 58 |         SX = sqrt(n + lambda)*SX + repmat(m,1,size(SX,2));
 59 | 
 60 |         % Propagate through the dynamic model
 61 |         HX = [SX(1,:)+SX(2,:)*DT; SX(2,:)-g*sin(SX(1,:))*DT];
 62 |   
 63 |         % Compute the predicted mean and covariance
 64 |         m = zeros(size(m));
 65 |         P = zeros(size(P));
 66 |         for i=1:size(HX,2)
 67 |             m = m + WM(i) * HX(:,i);
 68 |         end
 69 |         for i=1:size(HX,2)
 70 |             P = P + WC(i) * (HX(:,i) - m) * (HX(:,i) - m)';
 71 |         end
 72 |         P = P + Q;
 73 | 
 74 |         % Form sigma points for measurement step and
 75 |         % propagate throught the measurement model
 76 |         A = chol(P,'lower');
 77 |         SX = [zeros(size(m)) A -A];
 78 |         SX = sqrt(n + lambda)*SX + repmat(m,1,size(SX,2));
 79 |         HY = sin(SX(1,:)); 
 80 |     
 81 |         % Compute the updated mean and covariance
 82 |         mu = zeros(size(HY,1),1);
 83 |         S  = zeros(size(HY,1),size(HY,1));
 84 |         C  = zeros(size(SX,1),size(HY,1));
 85 |         for i=1:size(SX,2)
 86 |             mu = mu + WM(i) * HY(:,i);
 87 |         end
 88 |         for i=1:size(SX,2)
 89 |             S = S + WC(i) * (HY(:,i) - mu) * (HY(:,i) - mu)';
 90 |             C = C + WC(i) * (SX(:,i) - m) * (HY(:,i) - mu)';
 91 |         end
 92 |         S = S + R;
 93 |     
 94 |         % Compute the gain and updated mean and covariance  
 95 |         K = C/S;
 96 |         m = m + K*(Y(k) - mu);
 97 |         P = P - K*S*K';
 98 |     
 99 |         MM(:,k) = m;
100 |         PP(:,:,k) = P;
101 |     end  
102 |   
103 |     h = plot(T,Y,'k.',T,X(1,:),'r-',T,MM(1,:),'b--');
104 |     set(h,'Linewidth',5);
105 |     title('UKF estimate');
106 |     legend('Measurements','True','Estimate');
107 |   
108 |     rmse_ukf = sqrt(mean((X(1,:)-MM(1,:)).^2))
109 | 
110 | %%
111 | % Smoother
112 | %
113 | 
114 |     ms = m;
115 |     Ps = P;
116 |     MS = zeros(size(m,1),length(Y));
117 |     PS = zeros(size(P,1),size(P,2),length(Y));
118 |     MMS(:,k) = m;
119 |     PPS(:,:,k) = P;
120 |     for k=size(MM,2)-1:-1:1
121 |         m = MM(:,k);
122 |         P = PP(:,:,k);
123 |     
124 |         % Form the sigma points for dynamic model
125 |         A = chol(P)';
126 |         SX = [zeros(size(m)) A -A];
127 |         SX = sqrt(n + lambda)*SX + repmat(m,1,size(SX,2));
128 | 
129 |         % Propagate through the dynamic model
130 |         HX = [SX(1,:)+SX(2,:)*DT; SX(2,:)-g*sin(SX(1,:))*DT];
131 |   
132 |         % Compute the predicted mean and covariance
133 |         % and the cross-covariance D.
134 |         mp = zeros(size(m));
135 |         Pp = zeros(size(P));
136 |         D  = zeros(size(P));
137 |         for i=1:size(HX,2)
138 |             mp = mp + WM(i) * HX(:,i);
139 |         end
140 |         for i=1:size(HX,2)
141 |             Pp = Pp + WC(i) * (HX(:,i) - mp) * (HX(:,i) - mp)';
142 |             D = D + WC(i) * (SX(:,i) - m) * (HX(:,i) - mp)';
143 |         end
144 |         Pp = Pp + Q;
145 | 
146 |         Gk = D/Pp;
147 |         ms = m + Gk*(ms - mp);
148 |         Ps = P + Gk*(Ps - Pp)*Gk';
149 |         MMS(:,k) = ms;
150 |         PPS(:,:,k) = Ps;
151 |     end
152 |   
153 |     h = plot(T,Y,'k.',T,X(1,:),'r-',...
154 |              T,MM(1,:),'b--',T,MMS(1,:),'g--');
155 |     set(h,'Linewidth',5);
156 |     title('UKF and URTS estimates');
157 |     legend('Measurements','True','UKF','URTS');
158 |     
159 |     rmse_urts = sqrt(mean((X(1,:)-MMS(1,:)).^2))
160 |     
161 |     
162 | %%
163 | % Plot the filtering result
164 | %
165 | 
166 |     clf;
167 |     h=plot(T,X(1,:),'k',T,Y,'bo',T,MM(1,:),'r');
168 |   
169 |     legend('True angle','Measurements','UKF estimate');
170 |     xlabel('Time{\it t}');
171 |     ylabel('Pendulum angle {\it{x}}_{1,{\it{k}}}') 
172 |     
173 | 
174 | %%
175 | % Plot the smoothing result
176 | %
177 | 
178 |     clf;
179 |     h=plot(T,X(1,:),'k',T,Y,'bo',T,MMS(1,:),'r');
180 |   
181 |     legend('True angle','Measurements','URTSS estimate');
182 |     xlabel('Time{\it t}');
183 |     ylabel('Pendulum angle {\it{x}}_{1,{\it{k}}}') 
184 |    
185 | 


--------------------------------------------------------------------------------
/matlab/examples/pendulum_ukf5.m:
--------------------------------------------------------------------------------
  1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | %
  3 | % Estimate pendulum state with UKF5 and URTS5 as in Examples 8.23 and 14.13
  4 | % of the book
  5 | %
  6 | % Simo Sarkka and Lennard Svensson (2023), Bayesian Filtering and Smoothing,
  7 | % 2nd ed., Cambridge University Press.
  8 | % 
  9 | % See LICENSE provided with the software.
 10 | %
 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 12 | 
 13 | %%
 14 | % Simulate data
 15 | %
 16 |     pendulum_sim;
 17 | 
 18 | %%
 19 | % Filter
 20 | %
 21 | 
 22 |     m = m0;
 23 |     P = P0;
 24 |     
 25 |     n = size(m,1);
 26 | 
 27 |     [W, XI] = ut5_ws(n);
 28 |     
 29 |     %
 30 |     % Do the filtering
 31 |     %  
 32 |     MM = zeros(size(m,1),length(Y));
 33 |     PP = zeros(size(P,1),size(P,2),length(Y));
 34 |     for k=1:length(Y)
 35 | 
 36 |         % Form the sigma points for dynamic model
 37 |         SX = repmat(m,1,size(XI,2)) + chol(P,'lower') * XI;
 38 | 
 39 |         % Propagate through the dynamic model
 40 |         HX = [SX(1,:)+SX(2,:)*DT; SX(2,:)-g*sin(SX(1,:))*DT];
 41 |   
 42 |         % Compute the predicted mean and covariance
 43 |         m = zeros(size(m));
 44 |         P = zeros(size(P));
 45 |         for i=1:size(HX,2)
 46 |             m = m + W(i) * HX(:,i);
 47 |         end
 48 |         for i=1:size(HX,2)
 49 |             P = P + W(i) * (HX(:,i) - m) * (HX(:,i) - m)';
 50 |         end
 51 |         P = P + Q;
 52 | 
 53 |         % Form sigma points for measurement step and
 54 |         % propagate throught the measurement model
 55 |         SX = repmat(m,1,size(XI,2)) + chol(P,'lower') * XI;
 56 |         HY = sin(SX(1,:)); 
 57 |     
 58 |         % Compute the updated mean and covariance
 59 |         mu = zeros(size(HY,1),1);
 60 |         S  = zeros(size(HY,1),size(HY,1));
 61 |         C  = zeros(size(SX,1),size(HY,1));
 62 |         for i=1:size(SX,2)
 63 |             mu = mu + W(i) * HY(:,i);
 64 |         end
 65 |         for i=1:size(SX,2)
 66 |             S = S + W(i) * (HY(:,i) - mu) * (HY(:,i) - mu)';
 67 |             C = C + W(i) * (SX(:,i) - m) * (HY(:,i) - mu)';
 68 |         end
 69 |         S = S + R;
 70 |     
 71 |         % Compute the gain and updated mean and covariance  
 72 |         K = C/S;
 73 |         m = m + K*(Y(k) - mu);
 74 |         P = P - K*S*K';
 75 |     
 76 |         MM(:,k) = m;
 77 |         PP(:,:,k) = P;
 78 |     end  
 79 |   
 80 |     subplot(2,1,1);
 81 |     h = plot(T,Y,'k.',T,X(1,:),'r-',T,MM(1,:),'b--');
 82 |     set(h,'Linewidth',5);
 83 |     title('UKF5 estimate');
 84 |     legend('Measurements','True','Estimate');
 85 | 
 86 |     subplot(2,1,2);
 87 |     h = plot(T,squeeze(PP(1,1,:)),'b--');
 88 |     
 89 |     rmse_ukf5 = sqrt(mean((X(1,:)-MM(1,:)).^2))
 90 | 
 91 | %%
 92 | % Smoother
 93 | %
 94 | 
 95 |     ms = m;
 96 |     Ps = P;
 97 |     MS = zeros(size(m,1),length(Y));
 98 |     PS = zeros(size(P,1),size(P,2),length(Y));
 99 |     MMS(:,k) = m;
100 |     PPS(:,:,k) = P;
101 |     for k=size(MM,2)-1:-1:1
102 |         m = MM(:,k);
103 |         P = PP(:,:,k);
104 |     
105 |         % Form the sigma points for dynamic model
106 |         SX = repmat(m,1,size(XI,2)) + chol(P,'lower') * XI;
107 | 
108 |         % Propagate through the dynamic model
109 |         HX = [SX(1,:)+SX(2,:)*DT; SX(2,:)-g*sin(SX(1,:))*DT];
110 |  
111 |         % Compute the predicted mean and covariance
112 |         % and the cross-covariance D.
113 |         mp = zeros(size(m));
114 |         Pp = zeros(size(P));
115 |         D  = zeros(size(P));
116 |         for i=1:size(HX,2)
117 |             mp = mp + W(i) * HX(:,i);
118 |         end
119 |         for i=1:size(HX,2)
120 |             Pp = Pp + W(i) * (HX(:,i) - mp) * (HX(:,i) - mp)';
121 |             D = D + W(i) * (SX(:,i) - m) * (HX(:,i) - mp)';
122 |         end
123 |         Pp = Pp + Q;
124 | 
125 |         Ck = D/Pp;
126 |         ms = m + Ck*(ms - mp);
127 |         Ps = P + Ck*(Ps - Pp)*Ck';
128 |         MMS(:,k) = ms;
129 |         PPS(:,:,k) = Ps;
130 |     end
131 |   
132 |     h = plot(T,Y,'k.',T,X(1,:),'r-',...
133 |              T,MM(1,:),'b--',T,MMS(1,:),'g--');
134 |     set(h,'Linewidth',5);
135 |     title('UKF5 and URTS5 estimates');
136 |     legend('Measurements','True','UKF5','URTS5');
137 |     
138 |     rmse_urts5 = sqrt(mean((X(1,:)-MMS(1,:)).^2))
139 |     
140 | %%
141 | % Plot the filtering result
142 | %
143 | 
144 |     clf;
145 |     h=plot(T,X(1,:),'k',T,Y,'bo',T,MM(1,:),'r');
146 |   
147 |     legend('True angle','Measurements','UKF5 estimate');
148 |     xlabel('Time{\it t}');
149 |     ylabel('Pendulum angle {\it{x}}_{1,{\it{k}}}') 
150 |     
151 | %%
152 | % Plot the smoothing result
153 | %
154 |     clf;
155 |     h=plot(T,X(1,:),'k',T,Y,'bo',T,MMS(1,:),'r');
156 |   
157 |     legend('True angle','Measurements','URTSS5 estimate');
158 |     xlabel('Time{\it t}');
159 |     ylabel('Pendulum angle {\it{x}}_{1,{\it{k}}}') 
160 | 


--------------------------------------------------------------------------------
/matlab/examples/recursive_linreg.m:
--------------------------------------------------------------------------------
  1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | %
  3 | % Recursive linear regression demonstration from Chapter 3 of the book
  4 | %
  5 | % Simo Sarkka and Lennart Svensson (2023), Bayesian Filtering and Smoothing,
  6 | % 2nd ed. Cambridge University Press. 
  7 | %
  8 | % See LICENSE provided with the software.
  9 | %
 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 11 | 
 12 | %%
 13 | % Simulate data
 14 | %
 15 | 
 16 | %    randn('state',12);  % Use this to get book's data
 17 |     rng(1, 'twister');
 18 |     
 19 |     dt = 0.01;
 20 |     sd = 0.1;
 21 |     t = (0:dt:1);
 22 |     x = 1 + 0.5*t;
 23 |     y = x + sd*randn(size(x));
 24 |     
 25 |     h = plot(t,y,'.',t,x);
 26 |     
 27 |     axis([0 1 0.5 2]);
 28 |     
 29 |     set(h,'Markersize',7);
 30 |     set(h,'LineWidth',4);
 31 |     set(h(1),'Color',[0.0 0.0 0.0]);
 32 |     set(h(2),'Color',[0.7 0.7 0.7]);
 33 |         
 34 |     h = legend('Measurement','True Signal');
 35 |     xlabel('{\it t}');
 36 |     ylabel('{\it y}');
 37 |       
 38 | %%
 39 | % Batch linear regression
 40 | %
 41 |   m0 = [0;0];
 42 |   P0 = 1*eye(2);
 43 |   n  = length(y);
 44 |   H  = [ones(length(t),1) t'];
 45 |   mb = inv(inv(P0) + 1/sd^2*H'*H)*(1/sd^2*H'*y'+inv(P0)*m0)
 46 |   Pb = inv(inv(P0) + 1/sd^2*H'*H);
 47 |   
 48 |   h = plot(t,y,'.',t,x,t,mb(1)+mb(2)*t,'-');
 49 | 
 50 |   axis([0 1 0.5 2]);
 51 |   
 52 |   set(h,'Markersize',7);
 53 |   set(h(2),'LineWidth',4);
 54 |   set(h(3),'LineWidth',1.5);
 55 |   set(h(1),'Color',[0.0 0.0 0.0]);
 56 |   set(h(2),'Color',[0.7 0.7 0.7]);
 57 |   set(h(3),'Color',[0.0 0.0 0.0]);
 58 |   
 59 |   h = legend('Measurement','True Signal','Estimate');
 60 |   xlabel('{\it t}');
 61 |   ylabel('{\it y}');
 62 |   
 63 | %%
 64 | % Recursive solution
 65 | %
 66 |   m = m0;
 67 |   P = P0;
 68 |   MM = zeros(size(m0,1),length(y));
 69 |   PP = zeros(size(P0,1),size(P0,1),length(y));
 70 |   for k=1:length(y)
 71 |       H = [1 t(k)];
 72 |       S = H*P*H'+sd^2;
 73 |       K = P*H'/S;
 74 |       m = m + K*(y(k)-H*m);
 75 |       P = P - K*S*K';
 76 |       
 77 |       MM(:,k) = m;
 78 |       PP(:,:,k) = P;
 79 |   end
 80 |   % Note that this last estimate is exatly the same as mb:
 81 |   m
 82 | 
 83 |   h = plot(t,MM(1,:),'b-',[0 1],[mb(1) mb(1)],'b--',...
 84 |            t,MM(2,:),'r-',[0 1],[mb(2) mb(2)],'r--');
 85 |   
 86 |   set(h,'Markersize',10);
 87 |   set(h,'LineWidth',2);
 88 |   set(h(1),'Color',[0.0 0.0 0.0]);
 89 |   set(h(2),'Color',[0.0 0.0 0.0]);
 90 |   set(h(3),'Color',[0.5 0.5 0.5]);
 91 |   set(h(4),'Color',[0.5 0.5 0.5]);
 92 | 
 93 |   
 94 |   h = legend('Recursive E[ {\it\theta}_1 ]','Batch E[ {\it\theta}_1 ]',...
 95 |          'Recursive E[ {\it\theta}_2 ]','Batch E[ {\it\theta}_2 ]','Location','SE');
 96 |   
 97 |   xlabel('{\it t}');
 98 |   ylabel('{\it y}');
 99 |   grid on;
100 | 
101 | %%
102 |   h = semilogy(t,squeeze(PP(1,1,:)),'b-',[0 1],[Pb(1,1) Pb(1,1)],'b--',...
103 |                t,squeeze(PP(2,2,:)),'r-',[0 1],[Pb(2,2) Pb(2,2)],'r--');
104 | 
105 |   set(h,'Markersize',10);
106 |   set(h,'LineWidth',2);
107 |   set(h(1:2),'Color',[0.0 0.0 0.0]);
108 |   set(h(3:4),'Color',[0.5 0.5 0.5]);
109 |   
110 |   h = legend('Recursive Var[ {\it\theta}_1 ]','Batch Var[ {\it\theta}_1 ]',...
111 |          'Recursive Var[ {\it\theta}_2 ]','Batch Var[ {\it\theta}_2 ]');     
112 |   
113 |   xlabel('{\it t}');
114 |   ylabel('{\it y}');
115 |   grid on;
116 |   
117 | 


--------------------------------------------------------------------------------
/matlab/examples/resampling.m:
--------------------------------------------------------------------------------
 1 | %RESAMPLING  Perform resampling of weighted samples to uniform weighting
 2 | %
 3 | % Syntax:
 4 | %   ind = resampling(W,scheme,M,normalize)
 5 | %
 6 | % In:
 7 | %           W - 1xN vector of positive weights
 8 | %      scheme - Resampling scheme 'multinomial', 'stratified', or
 9 | %              'systematic' (default: 'multinomial')
10 | %           M - Number of indices to draw (default: length(W))
11 | %   normalize - Whether weight should be normalized, if not, then they
12 | %               are assumed to already sum to 1 (default: true)
13 | %
14 | % Out:
15 | %   ind - 1xM vector of indices.
16 | %
17 | % Description:
18 | %   Perform resampling using a selected method. The function returns a
19 | %   vector of indices such that the resampled version of original (W,X)
20 | %   is given by (1/M,X(ind)). The resampling method options are
21 | %   'multinomial', 'stratified', or 'systematic'. These particular
22 | %   implementations are based on "An introduction to Sequential Monte Carlo"
23 | %   by Nicolas Chopin and Omiros Papaspiliopoulos, Springer, 2020 and they
24 | %   have computational complexities of O(N), where N = length(W).
25 | 
26 | % Copyright:
27 | %   Copyright (c) 2021 Simo Särkkä
28 | %
29 | % License:
30 | %   This software is provided under the MIT License. See the accompanying 
31 | %   LICENSE file for details.
32 | 
33 | function ind = resampling(W,scheme,M,normalize)
34 | 
35 |     if nargin < 2
36 |         scheme = [];
37 |     end
38 |     if nargin < 3
39 |         M = [];
40 |     end
41 |     if nargin < 4
42 |         normalize = [];
43 |     end
44 |     
45 |     if isempty(scheme)
46 |         scheme = 'multinomial';
47 |     end
48 |     if isempty(M)
49 |         M = length(W);
50 |     end
51 |     if isempty(normalize)
52 |         normalize = true;
53 |     end
54 | 
55 |     if normalize
56 |         W = W ./ sum(W);
57 |     end    
58 |     
59 |     if strcmp(scheme,'multinomial')
60 |         tmp = cumsum(-log(rand(1,M + 1)));
61 |         us = tmp(1:end-1) / tmp(end);
62 |     elseif strcmp(scheme,'stratified')
63 |         us = (rand(1,M) + (0:(M-1))) / M;
64 |     elseif strcmp(scheme,'systematic')
65 |         us = (rand + (0:(M-1))) / M;
66 |     else
67 |         error('Unknown resampling scheme %s.\n',scheme);
68 |     end
69 |     
70 |     m = 1;
71 |     s = W(1);
72 |     M = length(us);
73 |     ind = zeros(1,M);
74 |     for n=1:M
75 |         while s < us(n)
76 |             m = m + 1;
77 |             s = s + W(m);
78 |         end
79 |         ind(n) = m;
80 |     end
81 | end
82 | 
83 | 
84 | 


--------------------------------------------------------------------------------
/matlab/examples/sut_ws.m:
--------------------------------------------------------------------------------
 1 | function [WM, XI, WC] = sut_ws(n,alpha,beta,kappa)
 2 | %SUT_WS  Scaled unscented transform weights and unit sigma points
 3 | %
 4 | % Syntax:
 5 | %   [WM, XI, WC] = sut_ws(n,alpha,beta,kappa)
 6 | %
 7 | % In:
 8 | %   n     - Dimensionality of random variable
 9 | %   alpha - Transformation parameter  (optional, default 0.5)
10 | %   beta  - Transformation parameter  (optional, default 2)
11 | %   kappa - Transformation parameter  (optional, default 3-n)
12 | %
13 | % Out:
14 | %   WM - Weights for mean calculation
15 | %   XI - Unit sigma points
16 | %   WC - Weights for covariance calculation
17 | 
18 |     %
19 |     % Check which arguments are there
20 |     %
21 |     if nargin < 1
22 |         error('At least dimensionality n required.');
23 |     end
24 |     if nargin < 2
25 |         alpha = [];
26 |     end
27 |     if nargin < 3
28 |         beta = [];
29 |     end
30 |     if nargin < 4
31 |         kappa = [];
32 |     end
33 |     
34 |     %
35 |     % Apply default values
36 |     %
37 |     if isempty(alpha)
38 |         alpha = 1;
39 |     end
40 |     if isempty(beta)
41 |         beta = 2;
42 |     end
43 |     if isempty(kappa)
44 |         kappa = 3 - n;
45 |     end
46 | 
47 |     %
48 |     % Compute the weights
49 |     %
50 |     lambda = alpha^2 * (n + kappa) - n;        
51 |     WM = zeros(1,2*n+1);
52 |     WC = zeros(1,2*n+1);
53 |     for j=1:2*n+1
54 |         if j==1
55 |             wm = lambda / (n + lambda);
56 |             wc = lambda / (n + lambda) + (1 - alpha^2 + beta);
57 |         else
58 |             wm = 1 / (2 * (n + lambda));
59 |             wc = wm;
60 |         end
61 |         WM(j) = wm;
62 |         WC(j) = wc;
63 |     end
64 | 
65 |     %
66 |     % Form the unit sigma points
67 |     %
68 |     XI = sqrt(n + lambda) * [zeros(n,1) eye(n) -eye(n)];
69 | end
70 | 


--------------------------------------------------------------------------------
/matlab/examples/sym_set.m:
--------------------------------------------------------------------------------
 1 | function U = sym_set( n, gen )
 2 | % U = sym_set( n, gen )
 3 | 
 4 |     if nargin < 3
 5 |         nonzero = 0;
 6 |     end
 7 | 
 8 |     if isempty(gen)
 9 |         if nonzero
10 |             U = [];
11 |         else
12 |             U = zeros(n,1);
13 |         end
14 |     else
15 |         U = [];
16 |         
17 |         for i=1:n
18 |             u = zeros(n,1);
19 |             u(i) = gen(1);
20 | 
21 |             if length(gen) > 1
22 |                 if abs(gen(1) - gen(2)) < eps
23 |                     V = sym_set(n-i, gen(2:end));
24 |                     for j=1:size(V,2)
25 |                         u(i+1:end) = V(:,j);
26 |                         U = [U u -u];
27 |                     end
28 |                 else
29 |                     V = sym_set(n-1, gen(2:end));
30 |                     for j=1:size(V,2)
31 |                         u([1:i-1 i+1:end]) = V(:,j);
32 |                         U = [U u -u];
33 |                     end
34 |                 end
35 |             else
36 |                 U = [U u -u];
37 |             end
38 |         end
39 |     end
40 | end
41 | 
42 | 


--------------------------------------------------------------------------------
/matlab/examples/ut5_ws.m:
--------------------------------------------------------------------------------
 1 | function [ W, XI, u ] = ut5_ws(n)
 2 | % [ W, XI, u ] = ut5_ws(n)
 3 | %
 4 | % Return weights and sigma-points for 5th order
 5 | % UT for dimension n.
 6 | 
 7 |     % The weights and sigma-point from McNamee & Stenger
 8 |     I0 = 1;
 9 |     I2 = 1;
10 |     I4 = 3;
11 |     I22 = 1;
12 |     
13 |     u = sqrt(I4/I2);
14 |     
15 |     A0 = I0 - n*(I2/I4)^2 * (I4 - 0.5 * (n-1)*I22);
16 |     A1 = 0.5 * (I2/I4)^2*(I4 - (n-1)*I22);
17 |     A11 = 0.25*(I2/I4)^2*I22;
18 |  
19 |     U0 = sym_set(n,[]);
20 |     U1 = sym_set(n,[u]);
21 |     U2 = sym_set(n,[u u]);
22 |     
23 |     XI = [U0 U1 U2];
24 |     W  = [A0*ones(1,size(U0,2)) A1*ones(1,size(U1,2)) A11*ones(1,size(U2,2))];
25 | end
26 | 
27 | 


--------------------------------------------------------------------------------
/matlab/exercises/exercise6_4.m:
--------------------------------------------------------------------------------
  1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | %
  3 | % Supplemental Matlab code for Exercise 6.4 in the book
  4 | %
  5 | % Simo Sarkka and Lennart Svensson (2023), Bayesian Filtering and Smoothing,
  6 | % 2nd ed. Cambridge University Press. 
  7 | %
  8 | % See LICENSE provided with the software.
  9 | %
 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 11 | 
 12 | %% Generate data
 13 | 
 14 |   % Lock random seed
 15 |   rng(123);
 16 | 
 17 |   % Gaussian ranom draw, m is the mean and S the covariance
 18 |   gauss_rnd = @(m,S) m + chol(S)'*randn(size(m));
 19 |   
 20 |   % Calculate root mean squared error
 21 |   rmse = @(x,y) sqrt(mean((x(:)-y(:)).^2));
 22 |   
 23 |   % Define parameters
 24 |   steps = 100;  % Number of time steps
 25 |   w     = 0.5;  % Angular velocity
 26 |   q     = 0.01; % Process noise spectral density
 27 |   r     = 0.1;  % Measurement noise variance
 28 | 
 29 |   % This is the transition matrix
 30 |   A = [cos(w)    sin(w)/w; 
 31 |        -w*sin(w) cos(w)];
 32 | 
 33 |   % This is the process noise covariance
 34 |   Q = [0.5*q*(w-cos(w)*sin(w))/w^3 0.5*q*sin(w)^2/w^2;
 35 |        0.5*q*sin(w)^2/w^2          0.5*q*(w+cos(w)*sin(w))/w];
 36 | 
 37 |   % This is the true initial value
 38 |   x0 = [0;0.1]; 
 39 | 
 40 |   % Simulate data
 41 |   X = zeros(2,steps);  % The true signal
 42 |   Y = zeros(1,steps);  % Measurements
 43 |   T = 1:steps;         % Time
 44 |   x = x0;
 45 |   for k=1:steps
 46 |     x = gauss_rnd(A*x,Q);
 47 |     y = gauss_rnd(x(1),r);
 48 |     X(:,k) = x;
 49 |     Y(:,k) = y;
 50 |   end
 51 | 
 52 |   % Visualize
 53 |   figure; clf;
 54 |     plot(T,X(1,:),'--',T,Y,'o');
 55 |     legend('True signal','Measurements');
 56 |     xlabel('Time step'); title('\bf Simulated data')
 57 |     
 58 |   
 59 | %% Baseline solution
 60 | 
 61 |   % Baseline solution. The estimates
 62 |   % of x_k are stored as columns of
 63 |   % the matrix EST1.
 64 |   
 65 |   % Calculate baseline estimate
 66 |   m1 = [0;1];  % Initialize first step with a guess
 67 |   EST1 = zeros(2,steps);
 68 |   for k=1:steps
 69 |     m1(2) = Y(k)-m1(1);
 70 |     m1(1) = Y(k);
 71 |     EST1(:,k) = m1;
 72 |   end
 73 | 
 74 |   % Visualize results
 75 |   figure; clf;
 76 |   
 77 |   % Plot the signal and its estimate
 78 |   subplot(2,1,1);
 79 |     plot(T,X(1,:),'--',T,EST1(1,:),'-',T,Y,'o');
 80 |     legend('True signal','Estimated signal','Measurements');
 81 |     xlabel('Time step'); title('\bf Baseline solution')
 82 |   
 83 |   % Plot the derivative and its estimate
 84 |   subplot(2,1,2);
 85 |     plot(T,X(2,:),'--',T,EST1(2,:),'-');
 86 |     legend('True derivative','Estimated derivative');
 87 |     xlabel('Time step')
 88 | 
 89 |   % Compute error
 90 |   err1 = rmse(X,EST1)
 91 |   
 92 |   
 93 | %% Kalman filter
 94 |   
 95 |   % Kalman filter solution. The estimates
 96 |   % of x_k are stored as columns of
 97 |   % the matrix EST2.
 98 | 
 99 |   m2 = [0;1];  % Initialize first step
100 |   P2 = eye(2); % Some uncertanty in covariance  
101 |   EST2 = zeros(2,steps); % Allocate space for results
102 | 
103 |   % Run Kalman filter
104 |   for k=1:steps
105 |     % TODO: Replace these with the Kalman filter equations
106 |     m2 = m2;
107 |     P2 = P2;
108 |     warning('You should add the Kalman filtering equations.')
109 |     
110 |     % Store the results
111 |     EST2(:,k) = m2;
112 |   end
113 | 
114 |   % Visualize results
115 |   figure; clf
116 |   
117 |   % Plot the signal and its estimate
118 |   subplot(2,1,1);
119 |     plot(T,X(1,:),'--',T,EST2(1,:),'-',T,Y,'o');
120 |     legend('True signal','Estimated signal','Measurements');
121 |     xlabel('Time step'); title('\bf Kalman filter')
122 |   
123 |   % Plot the derivative and its estimate
124 |   subplot(2,1,2);
125 |     plot(T,X(2,:),'--',T,EST2(2,:),'-');
126 |     legend('True derivative','Estimated derivative');
127 |     xlabel('Time step')
128 | 
129 |   % Compute error
130 |   err2 = rmse(X,EST2)
131 | 
132 | 


--------------------------------------------------------------------------------
/matlab/exercises/exercise7_2.m:
--------------------------------------------------------------------------------
  1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | %
  3 | % Supplemental Matlab code for Exercise 7.2 in the book
  4 | %
  5 | % Simo Sarkka and Lennart Svensson (2023), Bayesian Filtering and Smoothing,
  6 | % 2nd ed. Cambridge University Press. 
  7 | %
  8 | % See LICENSE provided with the software.
  9 | %
 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 11 | 
 12 | %% Generate data
 13 | 
 14 |   % Lock seed
 15 |   randn('state',123);
 16 | 
 17 |   % Implement RMSE (true and estimate)
 18 |   rmse = @(X,EST) sqrt(mean(sum((X-EST).^2)));
 19 |   
 20 |   % Create a bit curved trajectory and angle
 21 |   % measurements from two sensors
 22 |   S1 = [-1.5;0.5]; % Position of sensor 1
 23 |   S2 = [1;1];      % Position of sensor 2
 24 |   sd = 0.05;       % Standard deviation of measurements
 25 |   dt = 0.01;       % Sampling period
 26 |   x0 = [0;0;1;0];  % Initial state
 27 | 
 28 |   a = zeros(1,500);
 29 |   a(1,50:100)  = pi/2/51/dt + 0.01*randn(1,51);
 30 |   a(1,200:250) = pi/2/51/dt + 0.01*randn(1,51);
 31 |   a(1,350:400) = pi/2/51/dt + 0.01*randn(1,51);
 32 |   x = x0;
 33 |   t = 0;
 34 |   X = [];
 35 |   Theta = [];
 36 |   T = [];
 37 |   for i=1:500
 38 |     F = [0 0  1    0;...
 39 |          0 0  0    1;...
 40 |          0 0  0   a(i);...
 41 |          0 0 -a(i) 0];
 42 |     x = expm(F*dt)*x;
 43 |     y1 = atan2(x(2)-S1(2), x(1)-S1(1)) + sd * randn;
 44 |     y2 = atan2(x(2)-S2(2), x(1)-S2(1)) + sd * randn;
 45 |     t  = t + dt;
 46 |     X = [X x];
 47 |     T = [T t];
 48 |     Theta = [Theta [y1;y2]];
 49 |   end
 50 |   steps = size(Theta,2);
 51 | 
 52 | 
 53 | %% Dynamic model
 54 | 
 55 |   % Parameters of the dynamic model
 56 |   qc = 0.1;
 57 |   
 58 |   % This is the transition matrix
 59 |   A  = [1 0 dt 0;
 60 |         0 1 0 dt;
 61 |         0 0 1 0;
 62 |         0 0 0 1];
 63 | 
 64 |   % This is the process noise covariance
 65 |   Q = [qc*dt^3/3 0 qc*dt^2/2 0;
 66 |        0 qc*dt^3/3 0 qc*dt^2/2;
 67 |        qc*dt^2/2 0 qc*dt 0;
 68 |        0 qc*dt^2/2 0 qc*dt];
 69 |   
 70 |    
 71 | %% Baseline solution
 72 |    
 73 |   % Baseline solution. The estimates
 74 |   % of x_k are stored as columns of
 75 |   % the matrix EST1.
 76 | 
 77 |   %if 0 %% <--- Uncomment to disable
 78 |   
 79 |   fprintf('Running base line solution.\n');
 80 |     
 81 |   % Initialize to true value
 82 |   m1 = x0;     
 83 |   EST1 = zeros(4,steps);
 84 |   
 85 |   % Set up figure
 86 |   figure(1); clf
 87 |   
 88 |   % Loop through steps
 89 |   for k=1:steps
 90 |       
 91 |     % Compute crossing of the measurements
 92 |     dx1 = cos(Theta(1,k));
 93 |     dy1 = sin(Theta(1,k));
 94 |     dx2 = cos(Theta(2,k));
 95 |     dy2 = sin(Theta(2,k));
 96 |     d = [dx1 dx2; dy1 dy2]\[S2(1)-S1(1);S2(2)-S1(2)];
 97 |     
 98 |     % Crossing
 99 |     cross_xy = S1 + [dx1;dy1]*d(1);
100 |     
101 |     % Compute estimate
102 |     m1(3:4) = [0;0];
103 |     m1(1:2) = cross_xy;
104 |     EST1(:,k) = m1;
105 |     
106 |     % Animate
107 |     if rem(k,10) == 1
108 |       len = 3;
109 |       dx1 = len*cos(Theta(1,k));
110 |       dy1 = len*sin(Theta(1,k));
111 |       dx2 = len*cos(Theta(2,k));
112 |       dy2 = len*sin(Theta(2,k));
113 |       clf;
114 |       plot(X(1,:),X(2,:),'r-',...
115 |            m1(1),m1(2),'bo',...
116 |            EST1(1,1:k),EST1(2,1:k),'b--',...
117 |            [S1(1);S1(1)+dx1],[S1(2);S1(2)+dy1],'k--',...
118 |            [S2(1);S2(1)+dx2],[S2(2);S2(2)+dy2],'k--');
119 |       axis([-2 2 -2.5 1.5]);
120 |       
121 |       % Pause and draw
122 |       drawnow;
123 |       pause(.1)
124 |     end
125 |   end
126 |     
127 |   % Compute error
128 |   err1 = rmse(X,EST1)
129 | 
130 |   % Plot baseline
131 |   figure(1); clf
132 | 
133 |     plot(X(1,:),X(2,:),'--',...
134 |          EST1(1,:),EST1(2,:),'-',...
135 |          S1(1),S1(2),'kx',S2(1),S2(2),'ko')
136 |     legend('True trajectory','Baseline estimate','Sensor 1','Sensor 2');
137 |     xlabel('x'); ylabel('y'); title('\bf Baseline Solution')
138 |     axis([-2 2 -2.5 1.5]);
139 |   
140 |     fprintf('This is the BL solution. Press enter.\n');
141 |     pause;
142 |   
143 |   %end %% <--- Uncomment to disable
144 |  
145 | %% EKF solution
146 |   
147 |   % EKF solution. The estimates
148 |   % of x_k are stored as columns of
149 |   % the matrix EST2.
150 |   
151 |   %if 0 %% <--- Uncomment to disable
152 | 
153 |   fprintf('Running EKF solution.\n');
154 | 
155 |   m2 = x0;            % Initialize to true value
156 |   P2 = eye(4);        % Some uncertainty
157 |   R  = sd^2*eye(2);   % The joint covariance
158 |   EST2 = zeros(4,steps);
159 |   
160 |   % Set up figure
161 |   figure(1); clf
162 |   
163 |   % Loop through steps
164 |   for k=1:steps
165 |     
166 |     % Compute estimate here
167 |     warning('You should implement EKF here.')
168 |     m2 = [0;0;0;0];
169 |     EST2(:,k) = m2;
170 |     
171 |     % Animate
172 |     if rem(k,10) == 1
173 |       len = 3;
174 |       dx1 = len*cos(Theta(1,k));
175 |       dy1 = len*sin(Theta(1,k));
176 |       dx2 = len*cos(Theta(2,k));
177 |       dy2 = len*sin(Theta(2,k));
178 |       clf;
179 |       plot(X(1,:),X(2,:),'r-',...
180 |            m2(1),m2(2),'bo',...
181 |            EST2(1,1:k),EST2(2,1:k),'b--',...
182 |            [S1(1);S1(1)+dx1],[S1(2);S1(2)+dy1],'k--',...
183 |            [S2(1);S2(1)+dx2],[S2(2);S2(2)+dy2],'k--');
184 |       axis([-2 2 -2.5 1.5]);
185 |       
186 |       % Pause and draw
187 |       drawnow;
188 |       pause(.1)
189 |     end
190 |   end
191 | 
192 |   % Compute error
193 |   err2 = rmse(X,EST2)
194 |   
195 |   % Plot EKF
196 |   figure(1); clf
197 | 
198 |     plot(X(1,:),X(2,:),'--',...
199 |          EST2(1,:),EST2(2,:),'-',...
200 |          S1(1),S1(2),'kx',S2(1),S2(2),'ko')
201 |     legend('True trajectory','EKF estimate','Sensor 1','Sensor 2');
202 |     xlabel('x'); ylabel('y'); title('\bf EKF Solution')
203 |     axis([-2 2 -2.5 1.5]);
204 |   
205 |   
206 |   fprintf('This will be the EKF solution. Press enter.\n');
207 |   pause;
208 |   
209 |   %end %% <--- Uncomment to disable
210 | 
211 | 


--------------------------------------------------------------------------------
/python/common_utilities/__init__.py:
--------------------------------------------------------------------------------
1 | from .simulate import *
2 | from .random import *
3 | from .stats import *
4 | from .plot import *


--------------------------------------------------------------------------------
/python/common_utilities/plot.py:
--------------------------------------------------------------------------------
 1 | import matplotlib.pyplot as plt
 2 | import numpy as np
 3 | 
 4 | __all__ = ["plot_pendulum", "plot_car_trajectory"]
 5 | 
 6 | 
 7 | def plot_pendulum(timeline, y, x1, label1, x2=None, label2=None):
 8 |     fig, axes = plt.subplots(ncols=2, figsize=(22, 10))
 9 |     axes[1].scatter(timeline, y, marker="o", label="Measurements", color="red", alpha=0.66)
10 |     axes[1].plot(timeline, np.sin(x1[:, 0]), linestyle="dashdot", label=label1, color="blue")
11 | 
12 |     axes[0].plot(x1[:, 0], x1[:, 1], label=label1, color="blue")
13 | 
14 |     if x2 is not None:
15 |         axes[1].plot(timeline, np.sin(x2[:, 0]), linestyle="dashdot", label=label2, color="orange")
16 |         axes[0].plot(x2[:, 0], x2[:, 1], label=label2, color="orange")
17 |     else:
18 |         axes[0].scatter(x1[0, 0], x1[0, 1], marker="x", color="orange", s=500)
19 | 
20 |     axes[0].set_xlabel("$x_0(t)$")
21 |     axes[0].set_ylabel("$x_1(t)$")
22 | 
23 |     axes[1].set_xlabel("$t$")
24 |     axes[1].set_ylabel("$\sin(x_0(t))$")
25 | 
26 |     axes[0].legend()
27 |     axes[1].legend()
28 | 
29 | 
30 | def plot_car_trajectory(y, x1, label1, x2=None, label2=None):
31 |     fig, ax = plt.subplots(figsize=(10, 10))
32 |     ax.scatter(y[:, 0], y[:, 1], marker="o", label="Measurements", color="red")
33 |     ax.plot(x1[:, 0], x1[:, 1], label=label1, color="blue")
34 |     if x2 is None:
35 |         ax.scatter(x1[0, 0], x1[0, 1], marker="x", color="orange", s=500)
36 |     else:
37 |         ax.plot(x2[:, 0], x2[:, 1], label=label2, color="orange")
38 |     _ = ax.legend()
39 |     _ = ax.set_xlabel("${\it x}_1$")
40 |     _ = ax.set_ylabel("${\it x}_2$")
41 | 


--------------------------------------------------------------------------------
/python/common_utilities/random.py:
--------------------------------------------------------------------------------
 1 | """
 2 | A random utility file used for reproducibility across languages (Octave, Julia, etc). Enforces the use of
 3 | Mersen-Twister 1997 and of some algorithms.
 4 | """
 5 | import numpy as np
 6 | import numpy.random as rd
 7 | from scipy.special import erfcinv
 8 | 
 9 | __all__ = ["RandomState"]
10 | 
11 | SQRT_2 = 2 ** 0.5
12 | 
13 | 
14 | class RandomState(rd.RandomState):
15 |     """Enforces MT1997 and reproducibility with Matlab"""
16 | 
17 |     def __init__(self, seed=0):
18 |         if not isinstance(seed, int):
19 |             raise TypeError(f"seed must be an int, '{seed}' of type '{type(seed)}' was passed")
20 |         super(RandomState, self).__init__(seed)
21 | 
22 |     def randn(self, *args):
23 |         if args:
24 |             uniforms = self.rand(*args[::-1]).T
25 |         else:
26 |             uniforms = self.rand()
27 |         return SQRT_2 * erfcinv(2 * uniforms)
28 | 
29 |     def choice(self, n, k, p=None):
30 |         if p is None:
31 |             p = np.full(n, 1/n)
32 |         cs = np.cumsum(p)
33 |         cs = cs / cs[-1]
34 |         uniforms = self.rand(k)
35 |         return np.searchsorted(cs, uniforms, "left")
36 | 
37 | 


--------------------------------------------------------------------------------
/python/common_utilities/simulate.py:
--------------------------------------------------------------------------------
  1 | """
  2 | A collection of simulation functions for State Space Models.
  3 | """
  4 | import numpy as np
  5 | from .random import RandomState
  6 | 
  7 | __all__ = ["generate_ssm", "generate_pendulum"]
  8 | 
  9 | 
 10 | def _atleast2d(*args):
 11 |     return tuple(np.atleast_2d(elem) for elem in args)
 12 | 
 13 | 
 14 | def generate_ssm(m_0, A, Q, H, R, steps, random_state):
 15 |     """Samples from a state space model given parameters and a random state
 16 | 
 17 |     Parameters
 18 |     ----------
 19 |     m_0 : (M,) array_like
 20 |         Initial mean of the state
 21 |     P_0 : (M, M) array_like
 22 |         Initial covariance of the state
 23 |     A : (M, M) or (M, M) array_like
 24 |         Transition matrix
 25 |     Q : (M, M) array_like
 26 |         Transition covariance
 27 |     H : (M, N) array_like
 28 |         Observation matrix
 29 |     R : (N, N) array_like
 30 |         Observation covariance
 31 |     steps : int
 32 |         Number of steps simulated
 33 |     random_state : RandomState
 34 |         Random state used for pseudo-random numbers generation
 35 | 
 36 |     Returns
 37 |     -------
 38 |     states : (steps, M) ndarray
 39 |         The true states
 40 |     observations : (steps, N) ndarray
 41 |         The noisy observations
 42 | 
 43 |     Examples
 44 |     --------
 45 |     >>> M, N = 2, 1
 46 |     >>> m_0 = np.zeros(M)
 47 |     >>> P_0 = Q  = [[0.4, -0.2],
 48 |     ...             [-0.2, 0.5]]
 49 |     >>> A = np.zeros((M, M))
 50 |     >>> H = [0., 0.]
 51 |     >>> R = 0.5
 52 |     >>> states, observations = generate_ssm(m_0, P_0, A, Q, H, R, 10000, RandomState(5))
 53 |     >>> est_cov = np.cov(states, rowvar=False)
 54 |     >>> est_error = np.cov(observations, rowvar=False)
 55 |     >>> cov_close = np.allclose(est_cov, Q, atol=1e-2)
 56 |     >>> error_close = np.allclose(est_error, R, atol=1e-2)
 57 |     >>> cov_close & error_close
 58 |     True
 59 |     """
 60 |     if not isinstance(random_state, RandomState):
 61 |         raise TypeError(f"random_state must be an instance of {RandomState}, "
 62 |                         f"'{random_state}' of type '{type(random_state)}' was given")
 63 | 
 64 |     m_0 = np.atleast_1d(m_0)
 65 |     A, Q, H, R = _atleast2d(A, Q, H, R)
 66 | 
 67 |     M = m_0.shape[-1]
 68 |     N = R.shape[-1]
 69 |     states = np.empty((steps, M))
 70 |     observations = np.empty((steps, N))
 71 | 
 72 |     chol_Q = np.linalg.cholesky(Q)
 73 |     chol_R = np.linalg.cholesky(R)
 74 | 
 75 |     state = m_0
 76 |     for i in range(steps):
 77 |         state = A @ state + chol_Q @ random_state.randn(M)
 78 |         states[i, :] = state
 79 |         obs = H @ state + chol_R @ random_state.randn(N)
 80 |         observations[i, :] = obs
 81 | 
 82 |     return states, observations
 83 | 
 84 | 
 85 | def generate_pendulum(m_0, g, Q, dt, R, steps, random_state, cluttered_probability=0., clutter_range=(-2, 2)):
 86 |     """ Samples from a noisy pendulum submitted to a gravitational pull g a random state.
 87 |     The state represents the angle and the angular moment, the measurement is the sine of the angle:
 88 |     the horizontal position of the pendulum.
 89 | 
 90 |     Parameters
 91 |     ----------
 92 |     m_0 : (2,) array_like
 93 |         Initial mean of the state
 94 |     g : float
 95 |         Gravitational pull (g-force) in N/kg (earth is ~9.81)
 96 |     Q : (2, 2) array_like
 97 |         Transition covariance coming from the discretisation of the model
 98 |     dt : float
 99 |         Time between each measurement
100 |     R : float
101 |         Observation variance
102 |     steps : int
103 |         Number of steps simulated
104 |     random_state : RandomState
105 |         Random state used for pseudo-random numbers generation
106 |     cluttered_probability : float, optional
107 |         What are the chances that the observations are cluttered
108 |     clutter_range: tuple of float
109 |         When observation is cluttered, it's replaced by a uniform in this range
110 | 
111 | 
112 |     Returns
113 |     -------
114 |     timeline: (steps) ndarray
115 |         The observation times
116 |     states : (steps, M) ndarray
117 |         The true states
118 |     observations : (steps, N) ndarray
119 |         The noisy observations
120 |     """
121 |     if not isinstance(random_state, RandomState):
122 |         raise TypeError(f"random_state must be an instance of {RandomState}, "
123 |                         f"'{random_state}' of type '{type(random_state)}' was given")
124 | 
125 |     m_0 = np.atleast_1d(m_0)
126 |     Q = np.atleast_2d(Q)
127 | 
128 |     states = np.empty((steps, 2))
129 |     observations = np.empty(steps)
130 | 
131 |     chol_Q = np.linalg.cholesky(Q)
132 |     sqrt_R = np.sqrt(R)
133 | 
134 |     state = m_0
135 | 
136 |     for i in range(steps):
137 |         state = np.array([state[0] + dt * state[1],
138 |                           state[1] - g * dt * np.sin(state[0])])
139 |         state = state + chol_Q @ random_state.randn(2)
140 |         states[i, :] = state
141 | 
142 |         observations[i] = np.sin(state[0]) + sqrt_R * random_state.randn()
143 | 
144 |     if cluttered_probability > 0:
145 |         cluttered_ind = random_state.rand(steps) < cluttered_probability
146 |         clutter_multiplier = clutter_range[1] - clutter_range[0]
147 |         observations[cluttered_ind] = random_state.rand(
148 |             cluttered_ind.astype(np.int_).sum()) * clutter_multiplier - clutter_multiplier / 2.
149 | 
150 |     return np.arange(dt, (steps + 1) * dt, dt), states, observations
151 | 


--------------------------------------------------------------------------------
/python/common_utilities/stats.py:
--------------------------------------------------------------------------------
 1 | """
 2 | A stats utility file used to compute usual statistics
 3 | """
 4 | import numpy as np
 5 | 
 6 | __all__ = ["rmse"]
 7 | 
 8 | 
 9 | def rmse(x, y):
10 |     """Computes the RMSE between x and y along all their dimensions, x and y must have the same dimensions.
11 | 
12 |         Parameters
13 |         ----------
14 |         x : (L,...) array_like
15 |             Initial mean of the state
16 |         y : (L, ...) array_like
17 |             Initial covariance of the state
18 | 
19 |         Returns
20 |         -------
21 |         out : float
22 |             The RMSE
23 |     """
24 |     return np.sqrt(np.mean(np.sum(np.square(x - y), -1)))
25 | 


--------------------------------------------------------------------------------
/python/example_notebooks/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/EEA-sensors/Bayesian-Filtering-and-Smoothing/efc88d0de842ee6522c56d342fadf8876a60d7f5/python/example_notebooks/__init__.py


--------------------------------------------------------------------------------
/python/example_notebooks/car_track.json:
--------------------------------------------------------------------------------
   1 | {
   2 |   "T": [
   3 |     0.01,
   4 |     0.02,
   5 |     0.03,
   6 |     0.04,
   7 |     0.05,
   8 |     0.060000000000000005,
   9 |     0.07,
  10 |     0.08,
  11 |     0.09,
  12 |     0.099999999999999992,
  13 |     0.10999999999999999,
  14 |     0.11999999999999998,
  15 |     0.12999999999999998,
  16 |     0.13999999999999999,
  17 |     0.15,
  18 |     0.16,
  19 |     0.17,
  20 |     0.18000000000000002,
  21 |     0.19000000000000003,
  22 |     0.20000000000000004,
  23 |     0.21000000000000005,
  24 |     0.22000000000000006,
  25 |     0.23000000000000007,
  26 |     0.24000000000000007,
  27 |     0.25000000000000006,
  28 |     0.26000000000000006,
  29 |     0.27000000000000007,
  30 |     0.28000000000000008,
  31 |     0.29000000000000009,
  32 |     0.3000000000000001,
  33 |     0.31000000000000011,
  34 |     0.32000000000000012,
  35 |     0.33000000000000013,
  36 |     0.34000000000000014,
  37 |     0.35000000000000014,
  38 |     0.36000000000000015,
  39 |     0.37000000000000016,
  40 |     0.38000000000000017,
  41 |     0.39000000000000018,
  42 |     0.40000000000000019,
  43 |     0.4100000000000002,
  44 |     0.42000000000000021,
  45 |     0.43000000000000022,
  46 |     0.44000000000000022,
  47 |     0.45000000000000023,
  48 |     0.46000000000000024,
  49 |     0.47000000000000025,
  50 |     0.48000000000000026,
  51 |     0.49000000000000027,
  52 |     0.50000000000000022,
  53 |     0.51000000000000023,
  54 |     0.52000000000000024,
  55 |     0.53000000000000025,
  56 |     0.54000000000000026,
  57 |     0.55000000000000027,
  58 |     0.56000000000000028,
  59 |     0.57000000000000028,
  60 |     0.58000000000000029,
  61 |     0.5900000000000003,
  62 |     0.60000000000000031,
  63 |     0.61000000000000032,
  64 |     0.62000000000000033,
  65 |     0.63000000000000034,
  66 |     0.64000000000000035,
  67 |     0.65000000000000036,
  68 |     0.66000000000000036,
  69 |     0.67000000000000037,
  70 |     0.68000000000000038,
  71 |     0.69000000000000039,
  72 |     0.7000000000000004,
  73 |     0.71000000000000041,
  74 |     0.72000000000000042,
  75 |     0.73000000000000043,
  76 |     0.74000000000000044,
  77 |     0.75000000000000044,
  78 |     0.76000000000000045,
  79 |     0.77000000000000046,
  80 |     0.78000000000000047,
  81 |     0.79000000000000048,
  82 |     0.80000000000000049,
  83 |     0.8100000000000005,
  84 |     0.82000000000000051,
  85 |     0.83000000000000052,
  86 |     0.84000000000000052,
  87 |     0.85000000000000053,
  88 |     0.86000000000000054,
  89 |     0.87000000000000055,
  90 |     0.88000000000000056,
  91 |     0.89000000000000057,
  92 |     0.90000000000000058,
  93 |     0.91000000000000059,
  94 |     0.9200000000000006,
  95 |     0.9300000000000006,
  96 |     0.94000000000000061,
  97 |     0.95000000000000062,
  98 |     0.96000000000000063,
  99 |     0.97000000000000064,
 100 |     0.98000000000000065,
 101 |     0.99000000000000066,
 102 |     1.0000000000000007,
 103 |     1.0100000000000007,
 104 |     1.0200000000000007,
 105 |     1.0300000000000007,
 106 |     1.0400000000000007,
 107 |     1.0500000000000007,
 108 |     1.0600000000000007,
 109 |     1.0700000000000007,
 110 |     1.0800000000000007,
 111 |     1.0900000000000007,
 112 |     1.1000000000000008,
 113 |     1.1100000000000008,
 114 |     1.1200000000000008,
 115 |     1.1300000000000008,
 116 |     1.1400000000000008,
 117 |     1.1500000000000008,
 118 |     1.1600000000000008,
 119 |     1.1700000000000008,
 120 |     1.1800000000000008,
 121 |     1.1900000000000008,
 122 |     1.2000000000000008,
 123 |     1.2100000000000009,
 124 |     1.2200000000000009,
 125 |     1.2300000000000009,
 126 |     1.2400000000000009,
 127 |     1.2500000000000009,
 128 |     1.2600000000000009,
 129 |     1.2700000000000009,
 130 |     1.2800000000000009,
 131 |     1.2900000000000009,
 132 |     1.3000000000000009,
 133 |     1.3100000000000009,
 134 |     1.320000000000001,
 135 |     1.330000000000001,
 136 |     1.340000000000001,
 137 |     1.350000000000001,
 138 |     1.360000000000001,
 139 |     1.370000000000001,
 140 |     1.380000000000001,
 141 |     1.390000000000001,
 142 |     1.400000000000001,
 143 |     1.410000000000001,
 144 |     1.420000000000001,
 145 |     1.430000000000001,
 146 |     1.4400000000000011,
 147 |     1.4500000000000011,
 148 |     1.4600000000000011,
 149 |     1.4700000000000011,
 150 |     1.4800000000000011,
 151 |     1.4900000000000011,
 152 |     1.5000000000000011,
 153 |     1.5100000000000011,
 154 |     1.5200000000000011,
 155 |     1.5300000000000011,
 156 |     1.5400000000000011,
 157 |     1.5500000000000012,
 158 |     1.5600000000000012,
 159 |     1.5700000000000012,
 160 |     1.5800000000000012,
 161 |     1.5900000000000012,
 162 |     1.6000000000000012,
 163 |     1.6100000000000012,
 164 |     1.6200000000000012,
 165 |     1.6300000000000012,
 166 |     1.6400000000000012,
 167 |     1.6500000000000012,
 168 |     1.6600000000000013,
 169 |     1.6700000000000013,
 170 |     1.6800000000000013,
 171 |     1.6900000000000013,
 172 |     1.7000000000000013,
 173 |     1.7100000000000013,
 174 |     1.7200000000000013,
 175 |     1.7300000000000013,
 176 |     1.7400000000000013,
 177 |     1.7500000000000013,
 178 |     1.7600000000000013,
 179 |     1.7700000000000014,
 180 |     1.7800000000000014,
 181 |     1.7900000000000014,
 182 |     1.8000000000000014,
 183 |     1.8100000000000014,
 184 |     1.8200000000000014,
 185 |     1.8300000000000014,
 186 |     1.8400000000000014,
 187 |     1.8500000000000014,
 188 |     1.8600000000000014,
 189 |     1.8700000000000014,
 190 |     1.8800000000000014,
 191 |     1.8900000000000015,
 192 |     1.9000000000000015,
 193 |     1.9100000000000015,
 194 |     1.9200000000000015,
 195 |     1.9300000000000015,
 196 |     1.9400000000000015,
 197 |     1.9500000000000015,
 198 |     1.9600000000000015,
 199 |     1.9700000000000015,
 200 |     1.9800000000000015,
 201 |     1.9900000000000015,
 202 |     2.0000000000000013,
 203 |     2.0100000000000011,
 204 |     2.0200000000000009,
 205 |     2.0300000000000007,
 206 |     2.0400000000000005,
 207 |     2.0500000000000003,
 208 |     2.06,
 209 |     2.07,
 210 |     2.0799999999999996,
 211 |     2.0899999999999994,
 212 |     2.0999999999999992,
 213 |     2.109999999999999,
 214 |     2.1199999999999988,
 215 |     2.1299999999999986,
 216 |     2.1399999999999983,
 217 |     2.1499999999999981,
 218 |     2.1599999999999979,
 219 |     2.1699999999999977,
 220 |     2.1799999999999975,
 221 |     2.1899999999999973,
 222 |     2.1999999999999971,
 223 |     2.2099999999999969,
 224 |     2.2199999999999966,
 225 |     2.2299999999999964,
 226 |     2.2399999999999962,
 227 |     2.249999999999996,
 228 |     2.2599999999999958,
 229 |     2.2699999999999956,
 230 |     2.2799999999999954,
 231 |     2.2899999999999952,
 232 |     2.2999999999999949,
 233 |     2.3099999999999947,
 234 |     2.3199999999999945,
 235 |     2.3299999999999943,
 236 |     2.3399999999999941,
 237 |     2.3499999999999939,
 238 |     2.3599999999999937,
 239 |     2.3699999999999934,
 240 |     2.3799999999999932,
 241 |     2.389999999999993,
 242 |     2.3999999999999928,
 243 |     2.4099999999999926,
 244 |     2.4199999999999924,
 245 |     2.4299999999999922,
 246 |     2.439999999999992,
 247 |     2.4499999999999917,
 248 |     2.4599999999999915,
 249 |     2.4699999999999913,
 250 |     2.4799999999999911,
 251 |     2.4899999999999909,
 252 |     2.4999999999999907,
 253 |     2.5099999999999905,
 254 |     2.5199999999999902,
 255 |     2.52999999999999,
 256 |     2.53999999999999,
 257 |     2.5499999999999896,
 258 |     2.5599999999999894,
 259 |     2.5699999999999892,
 260 |     2.579999999999989,
 261 |     2.5899999999999888,
 262 |     2.5999999999999885,
 263 |     2.6099999999999883,
 264 |     2.6199999999999881,
 265 |     2.6299999999999879,
 266 |     2.6399999999999877,
 267 |     2.6499999999999875,
 268 |     2.6599999999999873,
 269 |     2.6699999999999871,
 270 |     2.6799999999999868,
 271 |     2.6899999999999866,
 272 |     2.6999999999999864,
 273 |     2.7099999999999862,
 274 |     2.719999999999986,
 275 |     2.7299999999999858,
 276 |     2.7399999999999856,
 277 |     2.7499999999999853,
 278 |     2.7599999999999851,
 279 |     2.7699999999999849,
 280 |     2.7799999999999847,
 281 |     2.7899999999999845,
 282 |     2.7999999999999843,
 283 |     2.8099999999999841,
 284 |     2.8199999999999839,
 285 |     2.8299999999999836,
 286 |     2.8399999999999834,
 287 |     2.8499999999999832,
 288 |     2.859999999999983,
 289 |     2.8699999999999828,
 290 |     2.8799999999999826,
 291 |     2.8899999999999824,
 292 |     2.8999999999999821,
 293 |     2.9099999999999819,
 294 |     2.9199999999999817,
 295 |     2.9299999999999815,
 296 |     2.9399999999999813,
 297 |     2.9499999999999811,
 298 |     2.9599999999999809,
 299 |     2.9699999999999807,
 300 |     2.9799999999999804,
 301 |     2.9899999999999802,
 302 |     2.99999999999998,
 303 |     3.00999999999998,
 304 |     3.0199999999999796,
 305 |     3.0299999999999794,
 306 |     3.0399999999999792,
 307 |     3.049999999999979,
 308 |     3.0599999999999787,
 309 |     3.0699999999999785,
 310 |     3.0799999999999783,
 311 |     3.0899999999999781,
 312 |     3.0999999999999779,
 313 |     3.1099999999999777,
 314 |     3.1199999999999775,
 315 |     3.1299999999999772,
 316 |     3.139999999999977,
 317 |     3.1499999999999768,
 318 |     3.1599999999999766,
 319 |     3.1699999999999764,
 320 |     3.1799999999999762,
 321 |     3.189999999999976,
 322 |     3.1999999999999758,
 323 |     3.2099999999999755,
 324 |     3.2199999999999753,
 325 |     3.2299999999999751,
 326 |     3.2399999999999749,
 327 |     3.2499999999999747,
 328 |     3.2599999999999745,
 329 |     3.2699999999999743,
 330 |     3.279999999999974,
 331 |     3.2899999999999738,
 332 |     3.2999999999999736,
 333 |     3.3099999999999734,
 334 |     3.3199999999999732,
 335 |     3.329999999999973,
 336 |     3.3399999999999728,
 337 |     3.3499999999999726,
 338 |     3.3599999999999723,
 339 |     3.3699999999999721,
 340 |     3.3799999999999719,
 341 |     3.3899999999999717,
 342 |     3.3999999999999715,
 343 |     3.4099999999999713,
 344 |     3.4199999999999711,
 345 |     3.4299999999999708,
 346 |     3.4399999999999706,
 347 |     3.4499999999999704,
 348 |     3.45999999999997,
 349 |     3.46999999999997,
 350 |     3.47999999999997,
 351 |     3.4899999999999696,
 352 |     3.4999999999999694,
 353 |     3.5099999999999691,
 354 |     3.5199999999999689,
 355 |     3.5299999999999687,
 356 |     3.5399999999999685,
 357 |     3.5499999999999683,
 358 |     3.5599999999999681,
 359 |     3.5699999999999679,
 360 |     3.5799999999999677,
 361 |     3.5899999999999674,
 362 |     3.5999999999999672,
 363 |     3.609999999999967,
 364 |     3.6199999999999668,
 365 |     3.6299999999999666,
 366 |     3.6399999999999664,
 367 |     3.6499999999999662,
 368 |     3.6599999999999659,
 369 |     3.6699999999999657,
 370 |     3.6799999999999655,
 371 |     3.6899999999999653,
 372 |     3.6999999999999651,
 373 |     3.7099999999999649,
 374 |     3.7199999999999647,
 375 |     3.7299999999999645,
 376 |     3.7399999999999642,
 377 |     3.749999999999964,
 378 |     3.7599999999999638,
 379 |     3.7699999999999636,
 380 |     3.7799999999999634,
 381 |     3.7899999999999632,
 382 |     3.799999999999963,
 383 |     3.8099999999999627,
 384 |     3.8199999999999625,
 385 |     3.8299999999999623,
 386 |     3.8399999999999621,
 387 |     3.8499999999999619,
 388 |     3.8599999999999617,
 389 |     3.8699999999999615,
 390 |     3.8799999999999613,
 391 |     3.889999999999961,
 392 |     3.8999999999999608,
 393 |     3.9099999999999606,
 394 |     3.9199999999999604,
 395 |     3.92999999999996,
 396 |     3.93999999999996,
 397 |     3.9499999999999598,
 398 |     3.9599999999999596,
 399 |     3.9699999999999593,
 400 |     3.9799999999999591,
 401 |     3.9899999999999589,
 402 |     3.9999999999999587,
 403 |     4.0099999999999589,
 404 |     4.0199999999999587,
 405 |     4.0299999999999585,
 406 |     4.0399999999999583,
 407 |     4.0499999999999581,
 408 |     4.0599999999999579,
 409 |     4.0699999999999577,
 410 |     4.0799999999999574,
 411 |     4.0899999999999572,
 412 |     4.099999999999957,
 413 |     4.1099999999999568,
 414 |     4.1199999999999566,
 415 |     4.1299999999999564,
 416 |     4.1399999999999562,
 417 |     4.1499999999999559,
 418 |     4.1599999999999557,
 419 |     4.1699999999999555,
 420 |     4.1799999999999553,
 421 |     4.1899999999999551,
 422 |     4.1999999999999549,
 423 |     4.2099999999999547,
 424 |     4.2199999999999545,
 425 |     4.2299999999999542,
 426 |     4.239999999999954,
 427 |     4.2499999999999538,
 428 |     4.2599999999999536,
 429 |     4.2699999999999534,
 430 |     4.2799999999999532,
 431 |     4.289999999999953,
 432 |     4.2999999999999527,
 433 |     4.3099999999999525,
 434 |     4.3199999999999523,
 435 |     4.3299999999999521,
 436 |     4.3399999999999519,
 437 |     4.3499999999999517,
 438 |     4.3599999999999515,
 439 |     4.3699999999999513,
 440 |     4.379999999999951,
 441 |     4.3899999999999508,
 442 |     4.3999999999999506,
 443 |     4.40999999999995,
 444 |     4.41999999999995,
 445 |     4.42999999999995,
 446 |     4.43999999999995,
 447 |     4.4499999999999496,
 448 |     4.4599999999999493,
 449 |     4.4699999999999491,
 450 |     4.4799999999999489,
 451 |     4.4899999999999487,
 452 |     4.4999999999999485,
 453 |     4.5099999999999483,
 454 |     4.5199999999999481,
 455 |     4.5299999999999478,
 456 |     4.5399999999999476,
 457 |     4.5499999999999474,
 458 |     4.5599999999999472,
 459 |     4.569999999999947,
 460 |     4.5799999999999468,
 461 |     4.5899999999999466,
 462 |     4.5999999999999464,
 463 |     4.6099999999999461,
 464 |     4.6199999999999459,
 465 |     4.6299999999999457,
 466 |     4.6399999999999455,
 467 |     4.6499999999999453,
 468 |     4.6599999999999451,
 469 |     4.6699999999999449,
 470 |     4.6799999999999446,
 471 |     4.6899999999999444,
 472 |     4.6999999999999442,
 473 |     4.709999999999944,
 474 |     4.7199999999999438,
 475 |     4.7299999999999436,
 476 |     4.7399999999999434,
 477 |     4.7499999999999432,
 478 |     4.7599999999999429,
 479 |     4.7699999999999427,
 480 |     4.7799999999999425,
 481 |     4.7899999999999423,
 482 |     4.7999999999999421,
 483 |     4.8099999999999419,
 484 |     4.8199999999999417,
 485 |     4.8299999999999415,
 486 |     4.8399999999999412,
 487 |     4.849999999999941,
 488 |     4.8599999999999408,
 489 |     4.8699999999999406,
 490 |     4.87999999999994,
 491 |     4.88999999999994,
 492 |     4.89999999999994,
 493 |     4.90999999999994,
 494 |     4.9199999999999395,
 495 |     4.9299999999999393,
 496 |     4.9399999999999391,
 497 |     4.9499999999999389,
 498 |     4.9599999999999387,
 499 |     4.9699999999999385,
 500 |     4.9799999999999383,
 501 |     4.989999999999938,
 502 |     4.9999999999999378
 503 |   ],
 504 |   "X": [
 505 |     [
 506 |       0.13260964743574111,
 507 |       0.30744036792791923,
 508 |       0.43817439676196168,
 509 |       0.5516835545400931,
 510 |       0.65910866664411416,
 511 |       0.72629297422879957,
 512 |       0.80888329038430551,
 513 |       0.86907966650056867,
 514 |       0.94236591037133133,
 515 |       1.0265492844541892,
 516 |       1.0852698415829791,
 517 |       1.1285270775754934,
 518 |       1.193407402667529,
 519 |       1.238751474991546,
 520 |       1.3062793318303429,
 521 |       1.3704311462796186,
 522 |       1.3838254175042781,
 523 |       1.3871806285822827,
 524 |       1.3909550859969906,
 525 |       1.4268942728130289,
 526 |       1.468567264174935,
 527 |       1.4611916478554232,
 528 |       1.4218235292828323,
 529 |       1.3692987770095637,
 530 |       1.3384145281542073,
 531 |       1.2896471584112037,
 532 |       1.2391626788810561,
 533 |       1.2105619783207568,
 534 |       1.1269871383174794,
 535 |       1.0158940942377244,
 536 |       0.88355950260331406,
 537 |       0.76516858219401751,
 538 |       0.64761450951157584,
 539 |       0.52068232586316188,
 540 |       0.374467650098735,
 541 |       0.19307237008017944,
 542 |       0.0043424056463365916,
 543 |       -0.15418937496717927,
 544 |       -0.30376703339027794,
 545 |       -0.46717206629453034,
 546 |       -0.57225564795873129,
 547 |       -0.66602279471335268,
 548 |       -0.761295543768938,
 549 |       -0.82769602670275455,
 550 |       -0.90585821409724432,
 551 |       -1.0202282009066481,
 552 |       -1.1273913349725055,
 553 |       -1.2522506663868809,
 554 |       -1.3936886895609031,
 555 |       -1.5097280834754432,
 556 |       -1.6170299513558841,
 557 |       -1.7303866053169121,
 558 |       -1.8686677844023629,
 559 |       -1.9717399452834836,
 560 |       -2.0272667227245753,
 561 |       -2.0443709252655715,
 562 |       -1.9966324216423115,
 563 |       -1.9468904912591674,
 564 |       -1.8650910384343082,
 565 |       -1.7615217778134855,
 566 |       -1.6625685289022583,
 567 |       -1.5708257682631324,
 568 |       -1.4634477134173043,
 569 |       -1.3678473940079001,
 570 |       -1.2391988371409264,
 571 |       -1.0818579792785683,
 572 |       -0.87887674361376955,
 573 |       -0.65306667418543662,
 574 |       -0.40657108728956981,
 575 |       -0.094480719214004028,
 576 |       0.28282968486518495,
 577 |       0.677676589726902,
 578 |       1.0604278782346601,
 579 |       1.4738682381906869,
 580 |       1.8476761082616731,
 581 |       2.245708382806757,
 582 |       2.6346538428575896,
 583 |       3.0061202681090378,
 584 |       3.3285121003874165,
 585 |       3.6276191153134287,
 586 |       3.8944674363843319,
 587 |       4.1632687639210264,
 588 |       4.451860574246183,
 589 |       4.749487069405216,
 590 |       5.054443026200353,
 591 |       5.3659514643353283,
 592 |       5.6908574717969156,
 593 |       6.0373964701763585,
 594 |       6.3533924628104765,
 595 |       6.6380079065148863,
 596 |       6.9455429463375218,
 597 |       7.2537173724310122,
 598 |       7.5726364056965609,
 599 |       7.9056984617799655,
 600 |       8.2115881031715414,
 601 |       8.5248974089150842,
 602 |       8.8808036432104487,
 603 |       9.2341111181483591,
 604 |       9.5887989643044573,
 605 |       9.9057055334059569
 606 |     ],
 607 |     [
 608 |       -0.13858033849695625,
 609 |       -0.2567226046439407,
 610 |       -0.35007138883043953,
 611 |       -0.43956232564337144,
 612 |       -0.57931760050615089,
 613 |       -0.77334218026499624,
 614 |       -0.96424490537982743,
 615 |       -1.1418537652487173,
 616 |       -1.33862478802816,
 617 |       -1.5245689882726468,
 618 |       -1.7021285564634372,
 619 |       -1.8991524777819793,
 620 |       -2.1490708040069291,
 621 |       -2.3757899882245028,
 622 |       -2.5968978637616003,
 623 |       -2.8095782938658345,
 624 |       -2.9762463739708127,
 625 |       -3.1093631760586646,
 626 |       -3.264705505155534,
 627 |       -3.425649342226234,
 628 |       -3.5587944515683354,
 629 |       -3.7472664522138683,
 630 |       -3.9218142996639314,
 631 |       -4.0916394732796233,
 632 |       -4.3005460338554613,
 633 |       -4.5286606019293387,
 634 |       -4.7461428743656437,
 635 |       -4.9100447263031226,
 636 |       -5.05650789545246,
 637 |       -5.1937790569504454,
 638 |       -5.2908840139590279,
 639 |       -5.4352804298399091,
 640 |       -5.566302510523375,
 641 |       -5.6587525661851785,
 642 |       -5.74212196887362,
 643 |       -5.8249434871330141,
 644 |       -5.9221683692929652,
 645 |       -6.0099993189863472,
 646 |       -6.10768168998668,
 647 |       -6.187897235962323,
 648 |       -6.2147496152333508,
 649 |       -6.224923296887682,
 650 |       -6.2901609977291155,
 651 |       -6.4303277950020066,
 652 |       -6.5851530392860935,
 653 |       -6.73611473361138,
 654 |       -6.9225898752489119,
 655 |       -7.12580812373775,
 656 |       -7.3216143279024077,
 657 |       -7.4921658333394436,
 658 |       -7.6829131518246436,
 659 |       -7.8407632590984,
 660 |       -7.9714820195944327,
 661 |       -8.0954391235746943,
 662 |       -8.2109477049201161,
 663 |       -8.3344823084043274,
 664 |       -8.42283039746551,
 665 |       -8.5354149470405289,
 666 |       -8.648267015807436,
 667 |       -8.7471207586302615,
 668 |       -8.8241494697545253,
 669 |       -8.9157423763495434,
 670 |       -9.0061222896107029,
 671 |       -9.1182131518245733,
 672 |       -9.24951695281398,
 673 |       -9.3524482774101578,
 674 |       -9.4085196731073459,
 675 |       -9.4295498716676782,
 676 |       -9.4410782798509,
 677 |       -9.44811346225057,
 678 |       -9.5007438155461141,
 679 |       -9.6063940379591752,
 680 |       -9.7185018870371618,
 681 |       -9.8418594035625482,
 682 |       -9.9534550137737643,
 683 |       -10.047551398204034,
 684 |       -10.111511218208797,
 685 |       -10.175880408697811,
 686 |       -10.189042509504546,
 687 |       -10.179682642833491,
 688 |       -10.182677008849724,
 689 |       -10.186763347046542,
 690 |       -10.190007850958812,
 691 |       -10.12089416198538,
 692 |       -10.026701124619557,
 693 |       -9.9234821494531147,
 694 |       -9.83817027193455,
 695 |       -9.7768495627689287,
 696 |       -9.7411464156750309,
 697 |       -9.7177158655991214,
 698 |       -9.7084139269336216,
 699 |       -9.7497092343046,
 700 |       -9.8449148347043156,
 701 |       -9.9309258106142853,
 702 |       -10.034581341638484,
 703 |       -10.1257838208414,
 704 |       -10.227301418207517,
 705 |       -10.356619235511738,
 706 |       -10.48271968601879,
 707 |       -10.595311183452404
 708 |     ],
 709 |     [
 710 |       1.5296451817190795,
 711 |       1.5061666113250047,
 712 |       1.3459292958472981,
 713 |       1.0544413531303545,
 714 |       0.76046326867274461,
 715 |       0.65896794110280765,
 716 |       0.596300217167801,
 717 |       0.75284694740018732,
 718 |       0.871480852882215,
 719 |       0.75926788041470572,
 720 |       0.11529672898900145,
 721 |       0.73698039332670917,
 722 |       0.4746205314324583,
 723 |       0.48721377886997652,
 724 |       0.61768387041656814,
 725 |       0.39475488710210449,
 726 |       -0.073378433756511707,
 727 |       -0.1166898354145352,
 728 |       0.5961719334765736,
 729 |       0.39522397625523131,
 730 |       0.18261127080477946,
 731 |       -0.49542109739440321,
 732 |       -0.39434492789293352,
 733 |       -0.38724965560869262,
 734 |       -0.4111491510713593,
 735 |       -0.60440385688094,
 736 |       -0.4976345339123045,
 737 |       -0.41508849973538875,
 738 |       -1.2875128805605482,
 739 |       -1.0022282993688134,
 740 |       -1.4131464307008219,
 741 |       -1.0454052199114743,
 742 |       -1.1814309875839473,
 743 |       -1.4393046525490374,
 744 |       -1.6208993980585618,
 745 |       -1.9459224173832912,
 746 |       -1.6647326961884255,
 747 |       -1.4687233671384543,
 748 |       -1.5237937092470142,
 749 |       -1.4024674283837661,
 750 |       -0.75148696596082876,
 751 |       -1.0325437980935805,
 752 |       -0.85354500010059164,
 753 |       -0.422797478600878,
 754 |       -0.95749385171936563,
 755 |       -1.172012338785293,
 756 |       -1.2080913688591308,
 757 |       -1.3610459782275242,
 758 |       -1.259896860138753,
 759 |       -1.2064573496336017,
 760 |       -0.95762361479051106,
 761 |       -1.2132304355229211,
 762 |       -1.3479566842289432,
 763 |       -0.8707856751117582,
 764 |       -0.56381984327352486,
 765 |       0.010698736702154599,
 766 |       0.41214807519865704,
 767 |       0.659776751937073,
 768 |       0.88323885176718753,
 769 |       0.89672523886137556,
 770 |       0.94566141071206045,
 771 |       0.98271117709138867,
 772 |       1.0484235417183392,
 773 |       1.0949343224447947,
 774 |       1.4838808908138645,
 775 |       1.8915422197533576,
 776 |       2.360480884180912,
 777 |       2.1475481049604661,
 778 |       2.5303116014501321,
 779 |       3.6641328868873364,
 780 |       3.8503226502704395,
 781 |       3.9263468797126477,
 782 |       3.9107554178220836,
 783 |       3.9501193487539239,
 784 |       3.6717134236463411,
 785 |       3.9189077864832087,
 786 |       3.9397384102419513,
 787 |       3.36350588326527,
 788 |       3.0421833390439876,
 789 |       2.9374987725937527,
 790 |       2.5679437180358482,
 791 |       2.81667769657209,
 792 |       3.0695912545464314,
 793 |       3.0334620749156884,
 794 |       3.036358188633463,
 795 |       3.234462879096859,
 796 |       3.4786608691797567,
 797 |       3.5607480769150923,
 798 |       2.9394436419274941,
 799 |       3.0172113418833497,
 800 |       3.0771184109750935,
 801 |       3.1879303099795937,
 802 |       3.3774767606987277,
 803 |       3.11487232166766,
 804 |       3.1378843352542143,
 805 |       3.4433427856419634,
 806 |       3.5953233264789053,
 807 |       3.5575685103367145,
 808 |       3.2279356715906182,
 809 |       3.158555995952077
 810 |     ],
 811 |     [
 812 |       -1.2261643764064716,
 813 |       -1.0913419824291839,
 814 |       -1.0030316117837184,
 815 |       -1.155385305289824,
 816 |       -1.8373209806773074,
 817 |       -1.9253852874099286,
 818 |       -1.9384900627133037,
 819 |       -1.860650303724813,
 820 |       -1.9097533320728408,
 821 |       -1.9464841951325282,
 822 |       -1.6884835168324333,
 823 |       -2.2124883516336014,
 824 |       -2.5300431587055789,
 825 |       -2.0470812047410236,
 826 |       -2.1657982792584347,
 827 |       -2.0563408252299218,
 828 |       -1.2772215422065156,
 829 |       -1.4266556089105096,
 830 |       -1.7109677037852105,
 831 |       -1.3386662059809717,
 832 |       -1.4353264206080427,
 833 |       -1.7413212506317564,
 834 |       -1.3625582053701817,
 835 |       -2.003441473682591,
 836 |       -2.1809830391343437,
 837 |       -2.2682589763978518,
 838 |       -1.9467888967318179,
 839 |       -1.4079242188968029,
 840 |       -1.601306325956277,
 841 |       -1.1221181280022678,
 842 |       -1.1928630181711308,
 843 |       -1.5515551533816161,
 844 |       -1.0379726041297936,
 845 |       -0.98028808724104166,
 846 |       -0.6203916882619237,
 847 |       -0.84412087849273465,
 848 |       -0.93294061457778121,
 849 |       -0.89777823984833394,
 850 |       -0.94997170333937675,
 851 |       -0.51011944236696394,
 852 |       -0.10306428722973277,
 853 |       -0.35467020015679207,
 854 |       -1.0860628808026229,
 855 |       -1.6760572137175682,
 856 |       -1.6637882913752675,
 857 |       -1.500055618180564,
 858 |       -2.0926546078970989,
 859 |       -2.0560958839535619,
 860 |       -1.7659267484969923,
 861 |       -2.0196759642718471,
 862 |       -1.885874420843944,
 863 |       -1.3572379425391357,
 864 |       -1.1141793781459812,
 865 |       -1.1890421875367037,
 866 |       -1.2327668248351624,
 867 |       -0.964530713477524,
 868 |       -0.99400506131517974,
 869 |       -1.0817522362189844,
 870 |       -0.98899193318310907,
 871 |       -0.92311624023389882,
 872 |       -0.75806318953958274,
 873 |       -1.0276630218765161,
 874 |       -0.85047348079859852,
 875 |       -1.1801594526046568,
 876 |       -1.3672182427844726,
 877 |       -0.81108365577538521,
 878 |       -0.27149977216329879,
 879 |       -0.0010852586921139551,
 880 |       -0.05806908688967638,
 881 |       -0.0702010159037755,
 882 |       -0.80870656421336984,
 883 |       -1.02843692107876,
 884 |       -1.3934141069067867,
 885 |       -1.1438636076080153,
 886 |       -1.1351993625363455,
 887 |       -0.67025762481161766,
 888 |       -0.54080205327759467,
 889 |       -0.48978262308189591,
 890 |       0.15408213385826075,
 891 |       0.31750911289897765,
 892 |       -0.00061656406816268383,
 893 |       -0.16242013799623456,
 894 |       0.28365611517974976,
 895 |       0.79502326353162889,
 896 |       1.1012439933485072,
 897 |       0.808344445266984,
 898 |       0.84120957540656316,
 899 |       0.532780129733723,
 900 |       0.24218820219272102,
 901 |       0.17100037400443208,
 902 |       -0.15534791573236334,
 903 |       -0.71409649153064869,
 904 |       -1.0115047018030709,
 905 |       -1.0742359151029062,
 906 |       -1.0555978038874894,
 907 |       -1.1101706283282216,
 908 |       -0.98893332063328021,
 909 |       -1.4168244062729149,
 910 |       -1.0852842124227378,
 911 |       -1.347211256357477
 912 |     ]
 913 |   ],
 914 |   "Y": [
 915 |     [
 916 |       -0.54664039786037555,
 917 |       1.8379265777660951,
 918 |       0.58929543970344211,
 919 |       2.1282529358142881,
 920 |       1.1460654509822945,
 921 |       1.3704792940695716,
 922 |       1.262840875530713,
 923 |       0.47322818323638227,
 924 |       1.8558518697345723,
 925 |       1.70935862546924,
 926 |       1.1821768633830516,
 927 |       0.86565864282699367,
 928 |       1.6942288706955804,
 929 |       1.4289148141755148,
 930 |       1.4967969731717572,
 931 |       1.8921473596800411,
 932 |       1.1858256440573953,
 933 |       1.36537771642418,
 934 |       1.699605419847473,
 935 |       1.8493396729503979,
 936 |       0.78402433340310729,
 937 |       1.5469204090014754,
 938 |       1.1274715879626054,
 939 |       1.9159646784006619,
 940 |       1.4527199335755565,
 941 |       1.9147941327924805,
 942 |       1.6888686735083749,
 943 |       1.4983373293230313,
 944 |       1.8163874154293018,
 945 |       0.1479170284238096,
 946 |       0.35205389654566477,
 947 |       0.7468710689978425,
 948 |       0.36564780395104485,
 949 |       -0.25099106303325081,
 950 |       0.53524825449010083,
 951 |       0.121057212477028,
 952 |       0.25921640302115023,
 953 |       0.12805232130199762,
 954 |       0.62213149381604915,
 955 |       -0.021163962945035419,
 956 |       -1.0297304367619868,
 957 |       -0.40695881566342956,
 958 |       -0.74318531838047019,
 959 |       -0.0895414317973885,
 960 |       -1.1414500791703701,
 961 |       -0.93888411583744324,
 962 |       -1.5768293902735806,
 963 |       -1.1207321474720455,
 964 |       -1.2672131226801624,
 965 |       -1.5875591218439147,
 966 |       -1.4746698940928387,
 967 |       -0.93440238301246836,
 968 |       -1.3805297280753999,
 969 |       -1.2793170580171829,
 970 |       -2.4942621520414665,
 971 |       -1.2893027911013857,
 972 |       -1.8514325981285267,
 973 |       -1.7248167206114318,
 974 |       -1.5145477066254212,
 975 |       -1.5563304939202924,
 976 |       -1.7303086864662536,
 977 |       -1.5908177623459494,
 978 |       -2.1081162399809377,
 979 |       -1.37335745012099,
 980 |       -1.2196073967707486,
 981 |       -0.855819293832811,
 982 |       -0.30836829025756141,
 983 |       -0.39030289164405757,
 984 |       -1.2067633796203681,
 985 |       -0.32980989776530117,
 986 |       0.52529131033658827,
 987 |       -0.29271016353782908,
 988 |       2.1117589184552408,
 989 |       2.4062121540331023,
 990 |       1.2157011240859954,
 991 |       2.3132633456972242,
 992 |       1.9735130821899618,
 993 |       3.77279292329352,
 994 |       3.8746118139923356,
 995 |       2.4604486383012363,
 996 |       3.4976654938206142,
 997 |       3.6304909558486416,
 998 |       3.9157388431663036,
 999 |       4.4448188728199307,
1000 |       4.6424481068278434,
1001 |       5.5480255959637494,
1002 |       5.4689014212290692,
1003 |       6.330248310872749,
1004 |       6.3454374629218249,
1005 |       7.5138604749300022,
1006 |       7.42575149537486,
1007 |       7.5267092761456267,
1008 |       6.5088243809843673,
1009 |       7.5203558597456688,
1010 |       7.95331193263273,
1011 |       9.0069083482176531,
1012 |       8.7456366343187781,
1013 |       10.03199411666955,
1014 |       8.9095847076042514,
1015 |       9.4638039999703256
1016 |     ],
1017 |     [
1018 |       -0.47096555534022178,
1019 |       -0.34982057858419185,
1020 |       0.18848089373980459,
1021 |       -0.19759566493718989,
1022 |       0.21823271421314072,
1023 |       -1.3902076657375351,
1024 |       -1.2129183906194534,
1025 |       -0.79537464369696331,
1026 |       -1.6904657726600765,
1027 |       -0.938625116887853,
1028 |       -2.25590275919904,
1029 |       -1.1802096331667939,
1030 |       -2.1197453674217845,
1031 |       -1.6260997141972173,
1032 |       -3.5873046410901672,
1033 |       -2.5249739255135353,
1034 |       -3.1033517778336428,
1035 |       -3.087639772782155,
1036 |       -2.6888810669706391,
1037 |       -3.782267964617108,
1038 |       -4.0119962628883616,
1039 |       -4.0618681731152853,
1040 |       -4.0498416341540029,
1041 |       -3.5289751737438957,
1042 |       -3.9582503853710693,
1043 |       -4.7635449277061319,
1044 |       -4.3090109794026823,
1045 |       -4.5341439647523192,
1046 |       -6.1407425135199105,
1047 |       -4.72835988597891,
1048 |       -4.9627512593095622,
1049 |       -5.1139105227029207,
1050 |       -4.8823932069705274,
1051 |       -5.0919071265738252,
1052 |       -5.335537444572175,
1053 |       -5.6392641541564359,
1054 |       -6.1232582028474942,
1055 |       -6.9572859815169039,
1056 |       -6.0475642362183937,
1057 |       -5.5628746428242053,
1058 |       -5.5123216061291789,
1059 |       -7.195270494021706,
1060 |       -5.8554248422453954,
1061 |       -7.4762908405878239,
1062 |       -6.9019603564482308,
1063 |       -6.6167336083325159,
1064 |       -6.0823822606541693,
1065 |       -6.5034955250404227,
1066 |       -7.2328192086129288,
1067 |       -6.90808308000408,
1068 |       -7.3017366644445545,
1069 |       -6.7241873241181516,
1070 |       -8.0973438644108384,
1071 |       -8.437064919042415,
1072 |       -8.5842112246188869,
1073 |       -8.1452127663334988,
1074 |       -8.29448719240503,
1075 |       -7.9497098234924071,
1076 |       -9.3789167721660309,
1077 |       -7.9444415781014683,
1078 |       -8.8087036480822771,
1079 |       -8.1914781449259735,
1080 |       -9.5492378926785175,
1081 |       -9.4132002398928929,
1082 |       -9.494880153435739,
1083 |       -9.2922433882796334,
1084 |       -9.8788394325981717,
1085 |       -10.050591490287031,
1086 |       -9.30062629397363,
1087 |       -9.0390697071077852,
1088 |       -10.413478835025174,
1089 |       -10.646605373189235,
1090 |       -10.048794958066336,
1091 |       -10.363267361728223,
1092 |       -10.131712082786747,
1093 |       -9.9512975732819466,
1094 |       -9.2511298835702149,
1095 |       -9.58234115265631,
1096 |       -9.8812431824353233,
1097 |       -10.641943123735167,
1098 |       -10.869546350569181,
1099 |       -11.22336352721342,
1100 |       -10.058455954736818,
1101 |       -9.8044091291926918,
1102 |       -10.566841247021507,
1103 |       -9.2945993823536721,
1104 |       -9.4270243935773159,
1105 |       -9.2737040123549423,
1106 |       -10.553924701139556,
1107 |       -8.3696385582278285,
1108 |       -9.5446540639756,
1109 |       -10.012670174260776,
1110 |       -10.064796521446068,
1111 |       -9.422129737731181,
1112 |       -9.6743052287632167,
1113 |       -10.17087439929859,
1114 |       -10.493032393565441,
1115 |       -10.34004086770406,
1116 |       -10.1230647125256,
1117 |       -10.644605433035178
1118 |     ]
1119 |   ]
1120 | }


--------------------------------------------------------------------------------
/python/example_notebooks/pendulum.json:
--------------------------------------------------------------------------------
   1 | {
   2 |   "T": [
   3 |     0.01,
   4 |     0.02,
   5 |     0.03,
   6 |     0.04,
   7 |     0.05,
   8 |     0.060000000000000005,
   9 |     0.07,
  10 |     0.08,
  11 |     0.09,
  12 |     0.099999999999999992,
  13 |     0.10999999999999999,
  14 |     0.11999999999999998,
  15 |     0.12999999999999998,
  16 |     0.13999999999999999,
  17 |     0.15,
  18 |     0.16,
  19 |     0.17,
  20 |     0.18000000000000002,
  21 |     0.19000000000000003,
  22 |     0.20000000000000004,
  23 |     0.21000000000000005,
  24 |     0.22000000000000006,
  25 |     0.23000000000000007,
  26 |     0.24000000000000007,
  27 |     0.25000000000000006,
  28 |     0.26000000000000006,
  29 |     0.27000000000000007,
  30 |     0.28000000000000008,
  31 |     0.29000000000000009,
  32 |     0.3000000000000001,
  33 |     0.31000000000000011,
  34 |     0.32000000000000012,
  35 |     0.33000000000000013,
  36 |     0.34000000000000014,
  37 |     0.35000000000000014,
  38 |     0.36000000000000015,
  39 |     0.37000000000000016,
  40 |     0.38000000000000017,
  41 |     0.39000000000000018,
  42 |     0.40000000000000019,
  43 |     0.4100000000000002,
  44 |     0.42000000000000021,
  45 |     0.43000000000000022,
  46 |     0.44000000000000022,
  47 |     0.45000000000000023,
  48 |     0.46000000000000024,
  49 |     0.47000000000000025,
  50 |     0.48000000000000026,
  51 |     0.49000000000000027,
  52 |     0.50000000000000022,
  53 |     0.51000000000000023,
  54 |     0.52000000000000024,
  55 |     0.53000000000000025,
  56 |     0.54000000000000026,
  57 |     0.55000000000000027,
  58 |     0.56000000000000028,
  59 |     0.57000000000000028,
  60 |     0.58000000000000029,
  61 |     0.5900000000000003,
  62 |     0.60000000000000031,
  63 |     0.61000000000000032,
  64 |     0.62000000000000033,
  65 |     0.63000000000000034,
  66 |     0.64000000000000035,
  67 |     0.65000000000000036,
  68 |     0.66000000000000036,
  69 |     0.67000000000000037,
  70 |     0.68000000000000038,
  71 |     0.69000000000000039,
  72 |     0.7000000000000004,
  73 |     0.71000000000000041,
  74 |     0.72000000000000042,
  75 |     0.73000000000000043,
  76 |     0.74000000000000044,
  77 |     0.75000000000000044,
  78 |     0.76000000000000045,
  79 |     0.77000000000000046,
  80 |     0.78000000000000047,
  81 |     0.79000000000000048,
  82 |     0.80000000000000049,
  83 |     0.8100000000000005,
  84 |     0.82000000000000051,
  85 |     0.83000000000000052,
  86 |     0.84000000000000052,
  87 |     0.85000000000000053,
  88 |     0.86000000000000054,
  89 |     0.87000000000000055,
  90 |     0.88000000000000056,
  91 |     0.89000000000000057,
  92 |     0.90000000000000058,
  93 |     0.91000000000000059,
  94 |     0.9200000000000006,
  95 |     0.9300000000000006,
  96 |     0.94000000000000061,
  97 |     0.95000000000000062,
  98 |     0.96000000000000063,
  99 |     0.97000000000000064,
 100 |     0.98000000000000065,
 101 |     0.99000000000000066,
 102 |     1.0000000000000007,
 103 |     1.0100000000000007,
 104 |     1.0200000000000007,
 105 |     1.0300000000000007,
 106 |     1.0400000000000007,
 107 |     1.0500000000000007,
 108 |     1.0600000000000007,
 109 |     1.0700000000000007,
 110 |     1.0800000000000007,
 111 |     1.0900000000000007,
 112 |     1.1000000000000008,
 113 |     1.1100000000000008,
 114 |     1.1200000000000008,
 115 |     1.1300000000000008,
 116 |     1.1400000000000008,
 117 |     1.1500000000000008,
 118 |     1.1600000000000008,
 119 |     1.1700000000000008,
 120 |     1.1800000000000008,
 121 |     1.1900000000000008,
 122 |     1.2000000000000008,
 123 |     1.2100000000000009,
 124 |     1.2200000000000009,
 125 |     1.2300000000000009,
 126 |     1.2400000000000009,
 127 |     1.2500000000000009,
 128 |     1.2600000000000009,
 129 |     1.2700000000000009,
 130 |     1.2800000000000009,
 131 |     1.2900000000000009,
 132 |     1.3000000000000009,
 133 |     1.3100000000000009,
 134 |     1.320000000000001,
 135 |     1.330000000000001,
 136 |     1.340000000000001,
 137 |     1.350000000000001,
 138 |     1.360000000000001,
 139 |     1.370000000000001,
 140 |     1.380000000000001,
 141 |     1.390000000000001,
 142 |     1.400000000000001,
 143 |     1.410000000000001,
 144 |     1.420000000000001,
 145 |     1.430000000000001,
 146 |     1.4400000000000011,
 147 |     1.4500000000000011,
 148 |     1.4600000000000011,
 149 |     1.4700000000000011,
 150 |     1.4800000000000011,
 151 |     1.4900000000000011,
 152 |     1.5000000000000011,
 153 |     1.5100000000000011,
 154 |     1.5200000000000011,
 155 |     1.5300000000000011,
 156 |     1.5400000000000011,
 157 |     1.5500000000000012,
 158 |     1.5600000000000012,
 159 |     1.5700000000000012,
 160 |     1.5800000000000012,
 161 |     1.5900000000000012,
 162 |     1.6000000000000012,
 163 |     1.6100000000000012,
 164 |     1.6200000000000012,
 165 |     1.6300000000000012,
 166 |     1.6400000000000012,
 167 |     1.6500000000000012,
 168 |     1.6600000000000013,
 169 |     1.6700000000000013,
 170 |     1.6800000000000013,
 171 |     1.6900000000000013,
 172 |     1.7000000000000013,
 173 |     1.7100000000000013,
 174 |     1.7200000000000013,
 175 |     1.7300000000000013,
 176 |     1.7400000000000013,
 177 |     1.7500000000000013,
 178 |     1.7600000000000013,
 179 |     1.7700000000000014,
 180 |     1.7800000000000014,
 181 |     1.7900000000000014,
 182 |     1.8000000000000014,
 183 |     1.8100000000000014,
 184 |     1.8200000000000014,
 185 |     1.8300000000000014,
 186 |     1.8400000000000014,
 187 |     1.8500000000000014,
 188 |     1.8600000000000014,
 189 |     1.8700000000000014,
 190 |     1.8800000000000014,
 191 |     1.8900000000000015,
 192 |     1.9000000000000015,
 193 |     1.9100000000000015,
 194 |     1.9200000000000015,
 195 |     1.9300000000000015,
 196 |     1.9400000000000015,
 197 |     1.9500000000000015,
 198 |     1.9600000000000015,
 199 |     1.9700000000000015,
 200 |     1.9800000000000015,
 201 |     1.9900000000000015,
 202 |     2.0000000000000013,
 203 |     2.0100000000000011,
 204 |     2.0200000000000009,
 205 |     2.0300000000000007,
 206 |     2.0400000000000005,
 207 |     2.0500000000000003,
 208 |     2.06,
 209 |     2.07,
 210 |     2.0799999999999996,
 211 |     2.0899999999999994,
 212 |     2.0999999999999992,
 213 |     2.109999999999999,
 214 |     2.1199999999999988,
 215 |     2.1299999999999986,
 216 |     2.1399999999999983,
 217 |     2.1499999999999981,
 218 |     2.1599999999999979,
 219 |     2.1699999999999977,
 220 |     2.1799999999999975,
 221 |     2.1899999999999973,
 222 |     2.1999999999999971,
 223 |     2.2099999999999969,
 224 |     2.2199999999999966,
 225 |     2.2299999999999964,
 226 |     2.2399999999999962,
 227 |     2.249999999999996,
 228 |     2.2599999999999958,
 229 |     2.2699999999999956,
 230 |     2.2799999999999954,
 231 |     2.2899999999999952,
 232 |     2.2999999999999949,
 233 |     2.3099999999999947,
 234 |     2.3199999999999945,
 235 |     2.3299999999999943,
 236 |     2.3399999999999941,
 237 |     2.3499999999999939,
 238 |     2.3599999999999937,
 239 |     2.3699999999999934,
 240 |     2.3799999999999932,
 241 |     2.389999999999993,
 242 |     2.3999999999999928,
 243 |     2.4099999999999926,
 244 |     2.4199999999999924,
 245 |     2.4299999999999922,
 246 |     2.439999999999992,
 247 |     2.4499999999999917,
 248 |     2.4599999999999915,
 249 |     2.4699999999999913,
 250 |     2.4799999999999911,
 251 |     2.4899999999999909,
 252 |     2.4999999999999907,
 253 |     2.5099999999999905,
 254 |     2.5199999999999902,
 255 |     2.52999999999999,
 256 |     2.53999999999999,
 257 |     2.5499999999999896,
 258 |     2.5599999999999894,
 259 |     2.5699999999999892,
 260 |     2.579999999999989,
 261 |     2.5899999999999888,
 262 |     2.5999999999999885,
 263 |     2.6099999999999883,
 264 |     2.6199999999999881,
 265 |     2.6299999999999879,
 266 |     2.6399999999999877,
 267 |     2.6499999999999875,
 268 |     2.6599999999999873,
 269 |     2.6699999999999871,
 270 |     2.6799999999999868,
 271 |     2.6899999999999866,
 272 |     2.6999999999999864,
 273 |     2.7099999999999862,
 274 |     2.719999999999986,
 275 |     2.7299999999999858,
 276 |     2.7399999999999856,
 277 |     2.7499999999999853,
 278 |     2.7599999999999851,
 279 |     2.7699999999999849,
 280 |     2.7799999999999847,
 281 |     2.7899999999999845,
 282 |     2.7999999999999843,
 283 |     2.8099999999999841,
 284 |     2.8199999999999839,
 285 |     2.8299999999999836,
 286 |     2.8399999999999834,
 287 |     2.8499999999999832,
 288 |     2.859999999999983,
 289 |     2.8699999999999828,
 290 |     2.8799999999999826,
 291 |     2.8899999999999824,
 292 |     2.8999999999999821,
 293 |     2.9099999999999819,
 294 |     2.9199999999999817,
 295 |     2.9299999999999815,
 296 |     2.9399999999999813,
 297 |     2.9499999999999811,
 298 |     2.9599999999999809,
 299 |     2.9699999999999807,
 300 |     2.9799999999999804,
 301 |     2.9899999999999802,
 302 |     2.99999999999998,
 303 |     3.00999999999998,
 304 |     3.0199999999999796,
 305 |     3.0299999999999794,
 306 |     3.0399999999999792,
 307 |     3.049999999999979,
 308 |     3.0599999999999787,
 309 |     3.0699999999999785,
 310 |     3.0799999999999783,
 311 |     3.0899999999999781,
 312 |     3.0999999999999779,
 313 |     3.1099999999999777,
 314 |     3.1199999999999775,
 315 |     3.1299999999999772,
 316 |     3.139999999999977,
 317 |     3.1499999999999768,
 318 |     3.1599999999999766,
 319 |     3.1699999999999764,
 320 |     3.1799999999999762,
 321 |     3.189999999999976,
 322 |     3.1999999999999758,
 323 |     3.2099999999999755,
 324 |     3.2199999999999753,
 325 |     3.2299999999999751,
 326 |     3.2399999999999749,
 327 |     3.2499999999999747,
 328 |     3.2599999999999745,
 329 |     3.2699999999999743,
 330 |     3.279999999999974,
 331 |     3.2899999999999738,
 332 |     3.2999999999999736,
 333 |     3.3099999999999734,
 334 |     3.3199999999999732,
 335 |     3.329999999999973,
 336 |     3.3399999999999728,
 337 |     3.3499999999999726,
 338 |     3.3599999999999723,
 339 |     3.3699999999999721,
 340 |     3.3799999999999719,
 341 |     3.3899999999999717,
 342 |     3.3999999999999715,
 343 |     3.4099999999999713,
 344 |     3.4199999999999711,
 345 |     3.4299999999999708,
 346 |     3.4399999999999706,
 347 |     3.4499999999999704,
 348 |     3.45999999999997,
 349 |     3.46999999999997,
 350 |     3.47999999999997,
 351 |     3.4899999999999696,
 352 |     3.4999999999999694,
 353 |     3.5099999999999691,
 354 |     3.5199999999999689,
 355 |     3.5299999999999687,
 356 |     3.5399999999999685,
 357 |     3.5499999999999683,
 358 |     3.5599999999999681,
 359 |     3.5699999999999679,
 360 |     3.5799999999999677,
 361 |     3.5899999999999674,
 362 |     3.5999999999999672,
 363 |     3.609999999999967,
 364 |     3.6199999999999668,
 365 |     3.6299999999999666,
 366 |     3.6399999999999664,
 367 |     3.6499999999999662,
 368 |     3.6599999999999659,
 369 |     3.6699999999999657,
 370 |     3.6799999999999655,
 371 |     3.6899999999999653,
 372 |     3.6999999999999651,
 373 |     3.7099999999999649,
 374 |     3.7199999999999647,
 375 |     3.7299999999999645,
 376 |     3.7399999999999642,
 377 |     3.749999999999964,
 378 |     3.7599999999999638,
 379 |     3.7699999999999636,
 380 |     3.7799999999999634,
 381 |     3.7899999999999632,
 382 |     3.799999999999963,
 383 |     3.8099999999999627,
 384 |     3.8199999999999625,
 385 |     3.8299999999999623,
 386 |     3.8399999999999621,
 387 |     3.8499999999999619,
 388 |     3.8599999999999617,
 389 |     3.8699999999999615,
 390 |     3.8799999999999613,
 391 |     3.889999999999961,
 392 |     3.8999999999999608,
 393 |     3.9099999999999606,
 394 |     3.9199999999999604,
 395 |     3.92999999999996,
 396 |     3.93999999999996,
 397 |     3.9499999999999598,
 398 |     3.9599999999999596,
 399 |     3.9699999999999593,
 400 |     3.9799999999999591,
 401 |     3.9899999999999589,
 402 |     3.9999999999999587,
 403 |     4.0099999999999589,
 404 |     4.0199999999999587,
 405 |     4.0299999999999585,
 406 |     4.0399999999999583,
 407 |     4.0499999999999581,
 408 |     4.0599999999999579,
 409 |     4.0699999999999577,
 410 |     4.0799999999999574,
 411 |     4.0899999999999572,
 412 |     4.099999999999957,
 413 |     4.1099999999999568,
 414 |     4.1199999999999566,
 415 |     4.1299999999999564,
 416 |     4.1399999999999562,
 417 |     4.1499999999999559,
 418 |     4.1599999999999557,
 419 |     4.1699999999999555,
 420 |     4.1799999999999553,
 421 |     4.1899999999999551,
 422 |     4.1999999999999549,
 423 |     4.2099999999999547,
 424 |     4.2199999999999545,
 425 |     4.2299999999999542,
 426 |     4.239999999999954,
 427 |     4.2499999999999538,
 428 |     4.2599999999999536,
 429 |     4.2699999999999534,
 430 |     4.2799999999999532,
 431 |     4.289999999999953,
 432 |     4.2999999999999527,
 433 |     4.3099999999999525,
 434 |     4.3199999999999523,
 435 |     4.3299999999999521,
 436 |     4.3399999999999519,
 437 |     4.3499999999999517,
 438 |     4.3599999999999515,
 439 |     4.3699999999999513,
 440 |     4.379999999999951,
 441 |     4.3899999999999508,
 442 |     4.3999999999999506,
 443 |     4.40999999999995,
 444 |     4.41999999999995,
 445 |     4.42999999999995,
 446 |     4.43999999999995,
 447 |     4.4499999999999496,
 448 |     4.4599999999999493,
 449 |     4.4699999999999491,
 450 |     4.4799999999999489,
 451 |     4.4899999999999487,
 452 |     4.4999999999999485,
 453 |     4.5099999999999483,
 454 |     4.5199999999999481,
 455 |     4.5299999999999478,
 456 |     4.5399999999999476,
 457 |     4.5499999999999474,
 458 |     4.5599999999999472,
 459 |     4.569999999999947,
 460 |     4.5799999999999468,
 461 |     4.5899999999999466,
 462 |     4.5999999999999464,
 463 |     4.6099999999999461,
 464 |     4.6199999999999459,
 465 |     4.6299999999999457,
 466 |     4.6399999999999455,
 467 |     4.6499999999999453,
 468 |     4.6599999999999451,
 469 |     4.6699999999999449,
 470 |     4.6799999999999446,
 471 |     4.6899999999999444,
 472 |     4.6999999999999442,
 473 |     4.709999999999944,
 474 |     4.7199999999999438,
 475 |     4.7299999999999436,
 476 |     4.7399999999999434,
 477 |     4.7499999999999432,
 478 |     4.7599999999999429,
 479 |     4.7699999999999427,
 480 |     4.7799999999999425,
 481 |     4.7899999999999423,
 482 |     4.7999999999999421,
 483 |     4.8099999999999419,
 484 |     4.8199999999999417,
 485 |     4.8299999999999415,
 486 |     4.8399999999999412,
 487 |     4.849999999999941,
 488 |     4.8599999999999408,
 489 |     4.8699999999999406,
 490 |     4.87999999999994,
 491 |     4.88999999999994,
 492 |     4.89999999999994,
 493 |     4.90999999999994,
 494 |     4.9199999999999395,
 495 |     4.9299999999999393,
 496 |     4.9399999999999391,
 497 |     4.9499999999999389,
 498 |     4.9599999999999387,
 499 |     4.9699999999999385,
 500 |     4.9799999999999383,
 501 |     4.989999999999938,
 502 |     4.9999999999999378
 503 |   ],
 504 |   "X": [
 505 |     [
 506 |       1.4999625291727958,
 507 |       1.4989227751135819,
 508 |       1.4967979162931166,
 509 |       1.4937212397779644,
 510 |       1.4895534371694321,
 511 |       1.4844173988924072,
 512 |       1.4782779894331197,
 513 |       1.4713314719940749,
 514 |       1.4634069558767682,
 515 |       1.4544808454761633,
 516 |       1.4446248483789972,
 517 |       1.4338158638353053,
 518 |       1.4220354605985122,
 519 |       1.4093222315733718,
 520 |       1.3956416974170331,
 521 |       1.3809990990017871,
 522 |       1.3653754376277949,
 523 |       1.348739403613713,
 524 |       1.3309518097455322,
 525 |       1.3120856892893871,
 526 |       1.2921897643879217,
 527 |       1.2712609638260186,
 528 |       1.2494247515876953,
 529 |       1.226625264467186,
 530 |       1.2028141114426376,
 531 |       1.1781519070820967,
 532 |       1.1526449849426479,
 533 |       1.1262425439033095,
 534 |       1.0990164860600802,
 535 |       1.0708202466756809,
 536 |       1.0417054253190607,
 537 |       1.0116627959965405,
 538 |       0.98072510857414585,
 539 |       0.94912843137123215,
 540 |       0.91689503327741773,
 541 |       0.88386655099711109,
 542 |       0.85000684967724016,
 543 |       0.815430424514822,
 544 |       0.78008901331157687,
 545 |       0.743935995814169,
 546 |       0.70714194296144162,
 547 |       0.66971333979125292,
 548 |       0.63166150870679616,
 549 |       0.59283918946854253,
 550 |       0.55354632173263685,
 551 |       0.513728433306105,
 552 |       0.47319169465325928,
 553 |       0.43203484066741016,
 554 |       0.39038984329398418,
 555 |       0.34834249998742856,
 556 |       0.30579208957338089,
 557 |       0.26294990287688774,
 558 |       0.21986628397333735,
 559 |       0.17653058701853458,
 560 |       0.13293222147402375,
 561 |       0.08922846347541695,
 562 |       0.045429888102400555,
 563 |       0.0015875492477222628,
 564 |       -0.042389998809658942,
 565 |       -0.086450409672661474,
 566 |       -0.13061446346046543,
 567 |       -0.17463151098527649,
 568 |       -0.21846412936197604,
 569 |       -0.26195643644699051,
 570 |       -0.30523026584584567,
 571 |       -0.34816853572966583,
 572 |       -0.39080976721900523,
 573 |       -0.43311679479125287,
 574 |       -0.47497417316351587,
 575 |       -0.51643696721541621,
 576 |       -0.55761679424503174,
 577 |       -0.59830465530607735,
 578 |       -0.638644140769043,
 579 |       -0.67836831033709688,
 580 |       -0.71743609493995419,
 581 |       -0.75575148727324593,
 582 |       -0.79337170186131878,
 583 |       -0.83029745467834315,
 584 |       -0.86654861321468091,
 585 |       -0.9020363826410408,
 586 |       -0.93676660893693076,
 587 |       -0.9706614233671933,
 588 |       -1.0037281503594031,
 589 |       -1.0359273482407059,
 590 |       -1.0673109318306095,
 591 |       -1.0977641728137619,
 592 |       -1.1272897856240163,
 593 |       -1.1559384078204138,
 594 |       -1.1836957574377194,
 595 |       -1.2105114812971507,
 596 |       -1.2364528774257946,
 597 |       -1.2613719620171959,
 598 |       -1.2853235798156317,
 599 |       -1.3082667783608486,
 600 |       -1.3302434157980241,
 601 |       -1.3512686880438498,
 602 |       -1.3714007825607404,
 603 |       -1.3906765860447872,
 604 |       -1.4088982660814724,
 605 |       -1.4261377528755221,
 606 |       -1.4424127788029155,
 607 |       -1.4576510605325916,
 608 |       -1.4719597554001014,
 609 |       -1.4853725836221803,
 610 |       -1.497891053744463,
 611 |       -1.5095294275753008,
 612 |       -1.5202827729921013,
 613 |       -1.5300265105610074,
 614 |       -1.538913437618852,
 615 |       -1.5468915042427551,
 616 |       -1.55389503731011,
 617 |       -1.559824742144573,
 618 |       -1.5646621840420369,
 619 |       -1.5683929574337017,
 620 |       -1.5711559404478421,
 621 |       -1.5729227351225781,
 622 |       -1.5737377089572262,
 623 |       -1.573629776489897,
 624 |       -1.5726149622196837,
 625 |       -1.5704677365925321,
 626 |       -1.56747705245424,
 627 |       -1.5636719490776974,
 628 |       -1.558911777886979,
 629 |       -1.5531359914579759,
 630 |       -1.5464146984541913,
 631 |       -1.5386832536115382,
 632 |       -1.5299362488213162,
 633 |       -1.5203004158752047,
 634 |       -1.5097131481641206,
 635 |       -1.4981428081897445,
 636 |       -1.4855484075356191,
 637 |       -1.4720387275981417,
 638 |       -1.4575735378162153,
 639 |       -1.4423182094637317,
 640 |       -1.4260968797370619,
 641 |       -1.4087866544589509,
 642 |       -1.3904934775980085,
 643 |       -1.3714054045779289,
 644 |       -1.3515356698294911,
 645 |       -1.3308864266647706,
 646 |       -1.3092414737671252,
 647 |       -1.2865318013537548,
 648 |       -1.2629320733584828,
 649 |       -1.2383989859421329,
 650 |       -1.2130691111581744,
 651 |       -1.186651678820287,
 652 |       -1.1593748357951394,
 653 |       -1.1310729913895143,
 654 |       -1.1015780785704277,
 655 |       -1.0711769838150142,
 656 |       -1.040117439177991,
 657 |       -1.0083119317083429,
 658 |       -0.9755512864492073,
 659 |       -0.94195357963698823,
 660 |       -0.90760968747368664,
 661 |       -0.87252001659952028,
 662 |       -0.836576379631081,
 663 |       -0.79983437036368876,
 664 |       -0.76249229307938249,
 665 |       -0.724504947730576,
 666 |       -0.68585350582231364,
 667 |       -0.64651628742664946,
 668 |       -0.60649606332999839,
 669 |       -0.565974072329544,
 670 |       -0.52500668400230133,
 671 |       -0.4834765611238579,
 672 |       -0.44153088793537737,
 673 |       -0.399175576535022,
 674 |       -0.35645686524180936,
 675 |       -0.31330400198454883,
 676 |       -0.269798058623468,
 677 |       -0.22609591512318705,
 678 |       -0.18226385837870349,
 679 |       -0.13816599566297189,
 680 |       -0.093852919862602441,
 681 |       -0.0495605891947747,
 682 |       -0.0051465358003101817,
 683 |       0.039303761279193369,
 684 |       0.0836638403175337,
 685 |       0.12796525755679003,
 686 |       0.17218348954401363,
 687 |       0.21631331765276363,
 688 |       0.26025084028440515,
 689 |       0.30406391517884529,
 690 |       0.34772251578852525,
 691 |       0.39120786883265563,
 692 |       0.43449555188116673,
 693 |       0.47745506490793804,
 694 |       0.52005092784093954,
 695 |       0.56212703983000811,
 696 |       0.60377267306856186,
 697 |       0.64495627810023759,
 698 |       0.68557229901925554,
 699 |       0.72554511008035927,
 700 |       0.7647693283128163,
 701 |       0.80328008337470391,
 702 |       0.84104691407788512,
 703 |       0.8781588407409594,
 704 |       0.91449931934659057,
 705 |       0.95010883029982274,
 706 |       0.98483643738373083,
 707 |       1.0186424087257155,
 708 |       1.0515580019767612,
 709 |       1.0835676487522734,
 710 |       1.114726497427222,
 711 |       1.1450886869129586,
 712 |       1.1746591986714088,
 713 |       1.2032094038462098,
 714 |       1.2307874293398022,
 715 |       1.25753863991836,
 716 |       1.2833000720916119,
 717 |       1.3079499217296251,
 718 |       1.3316563095936054,
 719 |       1.3542405362911638,
 720 |       1.375847143596034,
 721 |       1.3964942219677017,
 722 |       1.4161051586070146,
 723 |       1.4347411678563362,
 724 |       1.4525558327043908,
 725 |       1.4694664262882404,
 726 |       1.4854261072467145,
 727 |       1.5004328815226986,
 728 |       1.5144539569288695,
 729 |       1.5275344205207191,
 730 |       1.53952473641903,
 731 |       1.550467566729983,
 732 |       1.5603193887691862,
 733 |       1.5691731032922567,
 734 |       1.5770408164140997,
 735 |       1.5839321677938574,
 736 |       1.589873707804482,
 737 |       1.594833055574477,
 738 |       1.5988521307096217,
 739 |       1.6019144411502919,
 740 |       1.6040317871871501,
 741 |       1.6053052307346072,
 742 |       1.6057103604565521,
 743 |       1.6052182903623944,
 744 |       1.6038941343689159,
 745 |       1.6015697951797776,
 746 |       1.5984139642563107,
 747 |       1.5941785296247606,
 748 |       1.588980182410207,
 749 |       1.5828814446662232,
 750 |       1.5759284756301353,
 751 |       1.5681685094698008,
 752 |       1.5594351945312912,
 753 |       1.5498606312472922,
 754 |       1.539322111506459,
 755 |       1.5276927710645871,
 756 |       1.5150990693012421,
 757 |       1.5015962270196548,
 758 |       1.4869934132364337,
 759 |       1.4713172673735158,
 760 |       1.4547683408476701,
 761 |       1.4373501507697817,
 762 |       1.4190652248684867,
 763 |       1.3998300293431587,
 764 |       1.3796937008490269,
 765 |       1.3586051875573446,
 766 |       1.3364996434078906,
 767 |       1.3133853730323337,
 768 |       1.2894091372248031,
 769 |       1.2644634378150543,
 770 |       1.238503103265125,
 771 |       1.2116122818664323,
 772 |       1.1837487759091474,
 773 |       1.1550547591524944,
 774 |       1.125412486328224,
 775 |       1.0948538924062146,
 776 |       1.0634607403801983,
 777 |       1.0311508736541968,
 778 |       0.99805099494197447,
 779 |       0.96402280579391109,
 780 |       0.92916421429229479,
 781 |       0.893592404137551,
 782 |       0.85714486014675617,
 783 |       0.82003615650900863,
 784 |       0.78235841595110533,
 785 |       0.74415738371620122,
 786 |       0.70527340014544837,
 787 |       0.66576794183429411,
 788 |       0.62558314991797437,
 789 |       0.58476781053473259,
 790 |       0.54320605041973058,
 791 |       0.50102218350244521,
 792 |       0.45834348243390438,
 793 |       0.415253119525539,
 794 |       0.37168561813984741,
 795 |       0.32769558877137889,
 796 |       0.28330674257434835,
 797 |       0.23871912571450021,
 798 |       0.19395760636607781,
 799 |       0.14902236835272042,
 800 |       0.10398261910441881,
 801 |       0.058892642601754544,
 802 |       0.013762350749244145,
 803 |       -0.031462852278189846,
 804 |       -0.076736589318158285,
 805 |       -0.12188795313951861,
 806 |       -0.16694651794186527,
 807 |       -0.21212493628637741,
 808 |       -0.25716467928750131,
 809 |       -0.3020026749644823,
 810 |       -0.34671464632945176,
 811 |       -0.391040040520988,
 812 |       -0.43512401835626091,
 813 |       -0.478861141251519,
 814 |       -0.5223168654276017,
 815 |       -0.56543620944647721,
 816 |       -0.6080484727008858,
 817 |       -0.64993037574318524,
 818 |       -0.69130944438896946,
 819 |       -0.73213451491034243,
 820 |       -0.77233413346409352,
 821 |       -0.81188634017327,
 822 |       -0.85085561934677911,
 823 |       -0.889125352463806,
 824 |       -0.92666010162949719,
 825 |       -0.96350000354292886,
 826 |       -0.9995179697036064,
 827 |       -1.0347700179749788,
 828 |       -1.0692365342079493,
 829 |       -1.1028127453428398,
 830 |       -1.1355061431578155,
 831 |       -1.1673418889358298,
 832 |       -1.1985315319214276,
 833 |       -1.2288282840894105,
 834 |       -1.2583070498426212,
 835 |       -1.2869775413840627,
 836 |       -1.3148299156734531,
 837 |       -1.3416322021308373,
 838 |       -1.367399078069061,
 839 |       -1.3921125846864002,
 840 |       -1.4159440401302306,
 841 |       -1.4389130233363032,
 842 |       -1.4609041351730103,
 843 |       -1.4818675724881589,
 844 |       -1.501731224867568,
 845 |       -1.520514354605351,
 846 |       -1.5383364338591143,
 847 |       -1.5552099808518347,
 848 |       -1.5710540791398808,
 849 |       -1.58588613299573,
 850 |       -1.5997613686514964,
 851 |       -1.6126354944966512,
 852 |       -1.6245125280934876,
 853 |       -1.6353670332469279,
 854 |       -1.6451815386429263,
 855 |       -1.6539762112254657,
 856 |       -1.6618229326770981,
 857 |       -1.6687438653756752,
 858 |       -1.6748524068263553,
 859 |       -1.6799798332018197,
 860 |       -1.6840931575671432,
 861 |       -1.6872121084073066,
 862 |       -1.6893170896218925,
 863 |       -1.6903496886337557,
 864 |       -1.6904056904865117,
 865 |       -1.6894414015772306,
 866 |       -1.6874880189868917,
 867 |       -1.6846246759451957,
 868 |       -1.6809515612313637,
 869 |       -1.6765034328477924,
 870 |       -1.671094454577295,
 871 |       -1.6646141265310141,
 872 |       -1.6571040298901922,
 873 |       -1.6486758282726419,
 874 |       -1.6392877229072462,
 875 |       -1.6289297773730664,
 876 |       -1.6176183891499403,
 877 |       -1.6053436591906691,
 878 |       -1.5920510113493087,
 879 |       -1.5778746868710092,
 880 |       -1.5626841256818527,
 881 |       -1.5465526138919343,
 882 |       -1.5294651562393511,
 883 |       -1.51141515938269,
 884 |       -1.4926237305408456,
 885 |       -1.4728237999997384,
 886 |       -1.4520642893434705,
 887 |       -1.4304354893661964,
 888 |       -1.407850984150214,
 889 |       -1.3841889127427411,
 890 |       -1.3595385992996247,
 891 |       -1.3339741346027254,
 892 |       -1.307420890290466,
 893 |       -1.2799809722317879,
 894 |       -1.2514863821791835,
 895 |       -1.2220376790369207,
 896 |       -1.1915539349195263,
 897 |       -1.1600665381377455,
 898 |       -1.127773881266769,
 899 |       -1.094367074294782,
 900 |       -1.0600726660641524,
 901 |       -1.0249490507208718,
 902 |       -0.98882920150497766,
 903 |       -0.95183956637370259,
 904 |       -0.91402584441447865,
 905 |       -0.8753396307749991,
 906 |       -0.83597296019831024,
 907 |       -0.795737690604751,
 908 |       -0.75472648942134579,
 909 |       -0.71304249626967475,
 910 |       -0.67068167254011057,
 911 |       -0.62749252127154587,
 912 |       -0.58366494680408287,
 913 |       -0.53913973620317912,
 914 |       -0.49410061746896661,
 915 |       -0.44861603277287626,
 916 |       -0.40266356774143347,
 917 |       -0.35621791486939908,
 918 |       -0.30940086301290842,
 919 |       -0.26208987531090167,
 920 |       -0.21451778456261833,
 921 |       -0.16675358994144648,
 922 |       -0.11872001899286076,
 923 |       -0.07061219418952544,
 924 |       -0.022481079845865105,
 925 |       0.025831299138992642,
 926 |       0.0741887235688805,
 927 |       0.12260799467430572,
 928 |       0.1710033915274605,
 929 |       0.21932336946773776,
 930 |       0.2675897786383869,
 931 |       0.31572619267539548,
 932 |       0.36359854827748622,
 933 |       0.41122589091221118,
 934 |       0.45860871527589842,
 935 |       0.50572448844566431,
 936 |       0.55246624470352068,
 937 |       0.59854888968679631,
 938 |       0.64386212009718358,
 939 |       0.68853750484490539,
 940 |       0.73260342744397866,
 941 |       0.77608865446224784,
 942 |       0.81895522513489971,
 943 |       0.86132367790285724,
 944 |       0.90296275113525815,
 945 |       0.9436700102583,
 946 |       0.9834389562564485,
 947 |       1.0224185421177356,
 948 |       1.0605929019569857,
 949 |       1.0979774265075557,
 950 |       1.1346922750556887,
 951 |       1.1706293828869176,
 952 |       1.2055548681203556,
 953 |       1.2395967714938185,
 954 |       1.2726848363164018,
 955 |       1.3048586734580514,
 956 |       1.3360248868453855,
 957 |       1.3662404012839409,
 958 |       1.3955879909398388,
 959 |       1.424141617991175,
 960 |       1.4518276230298559,
 961 |       1.4786503636505275,
 962 |       1.5044259047223141,
 963 |       1.5290809970862174,
 964 |       1.5527279787498127,
 965 |       1.5756074346465914,
 966 |       1.5973621846779618,
 967 |       1.6180183581775363,
 968 |       1.637645730674417,
 969 |       1.6562992593872155,
 970 |       1.6740299602385298,
 971 |       1.690803251600616,
 972 |       1.7065503250488971,
 973 |       1.7214693066105657,
 974 |       1.735437625706304,
 975 |       1.748390055665392,
 976 |       1.7602930132103662,
 977 |       1.7710441461602715,
 978 |       1.7809432599057298,
 979 |       1.7898387018113593,
 980 |       1.7978688324444017,
 981 |       1.8049105177220559,
 982 |       1.8108687507043204,
 983 |       1.8158509289608307,
 984 |       1.8198870597109555,
 985 |       1.8229651445471819,
 986 |       1.8250670385234466,
 987 |       1.8262357748506328,
 988 |       1.8266552669283806,
 989 |       1.8261649499504153,
 990 |       1.8246989681702956,
 991 |       1.8222339615083718,
 992 |       1.8189064318424017,
 993 |       1.8146962771582786,
 994 |       1.8094497536225718,
 995 |       1.8031970925338578,
 996 |       1.7958346375064085,
 997 |       1.7874550289562432,
 998 |       1.7779806310101076,
 999 |       1.7674668575411285,
1000 |       1.7560441169187464,
1001 |       1.7436934172372294,
1002 |       1.7302579403738487,
1003 |       1.7157871003238514,
1004 |       1.7002762305822074,
1005 |       1.6836249514062396
1006 |     ],
1007 |     [
1008 |       -0.097569052056644137,
1009 |       -0.20926033714893047,
1010 |       -0.31105347957005081,
1011 |       -0.40806570861181696,
1012 |       -0.51456692554420758,
1013 |       -0.62072580039334435,
1014 |       -0.698136967566443,
1015 |       -0.78168332152589548,
1016 |       -0.90070642779514865,
1017 |       -0.99293695657237857,
1018 |       -1.079038175694879,
1019 |       -1.1752067441207632,
1020 |       -1.2676521918451051,
1021 |       -1.3671580586107543,
1022 |       -1.46226433477158,
1023 |       -1.5677136502442346,
1024 |       -1.6634391272919371,
1025 |       -1.7671044691700806,
1026 |       -1.8792794463336626,
1027 |       -1.9879963192638561,
1028 |       -2.0945137852698887,
1029 |       -2.18606260928501,
1030 |       -2.2696348591645044,
1031 |       -2.3741030046153155,
1032 |       -2.4714092589344947,
1033 |       -2.5532726525280038,
1034 |       -2.6368075047939907,
1035 |       -2.7235536057391592,
1036 |       -2.8188796993197607,
1037 |       -2.9052154003872688,
1038 |       -2.9923034699897939,
1039 |       -3.0938736401039679,
1040 |       -3.1694032496880324,
1041 |       -3.234483899561384,
1042 |       -3.2943993798585551,
1043 |       -3.3806665867689714,
1044 |       -3.4604981742698948,
1045 |       -3.5265960897002935,
1046 |       -3.6130404147893249,
1047 |       -3.6812220415290891,
1048 |       -3.7409801512755827,
1049 |       -3.8061090243435998,
1050 |       -3.8762077727702229,
1051 |       -3.93782969086418,
1052 |       -3.9763301083925398,
1053 |       -4.0400551247335743,
1054 |       -4.1169886505755589,
1055 |       -4.158652518370034,
1056 |       -4.2024234439863282,
1057 |       -4.2454305219396851,
1058 |       -4.2922573359949023,
1059 |       -4.31282834357392,
1060 |       -4.3234817546697828,
1061 |       -4.3623704210598095,
1062 |       -4.3747913556130422,
1063 |       -4.3804664509158258,
1064 |       -4.3819416606423127,
1065 |       -4.3987849780285249,
1066 |       -4.3995664189811041,
1067 |       -4.4069165820559544,
1068 |       -4.4103450556525381,
1069 |       -4.3875402349605022,
1070 |       -4.3586397509648513,
1071 |       -4.330139890730158,
1072 |       -4.2944885502707475,
1073 |       -4.2626205547389109,
1074 |       -4.2315349674063363,
1075 |       -4.1836356633193965,
1076 |       -4.1432562069557681,
1077 |       -4.1098721871402546,
1078 |       -4.060452570591921,
1079 |       -4.0235403833852255,
1080 |       -3.9789563692021859,
1081 |       -3.9086240265390546,
1082 |       -3.8322205999605528,
1083 |       -3.7617037647342,
1084 |       -3.6936925357402677,
1085 |       -3.6228255664277493,
1086 |       -3.5452057236068439,
1087 |       -3.4709190122496061,
1088 |       -3.3975726104756303,
1089 |       -3.3077730178674281,
1090 |       -3.2218127734176,
1091 |       -3.1403439467180037,
1092 |       -3.0510679203668145,
1093 |       -2.9554827368828738,
1094 |       -2.8644116361579157,
1095 |       -2.7780516849037058,
1096 |       -2.6854451736474467,
1097 |       -2.592253230739523,
1098 |       -2.5017649314530632,
1099 |       -2.3917587075682905,
1100 |       -2.2977057992014021,
1101 |       -2.1979801266308971,
1102 |       -2.101660532366544,
1103 |       -2.0104917058687666,
1104 |       -1.9233356243109567,
1105 |       -1.8261671002307216,
1106 |       -1.722981086136028,
1107 |       -1.6299297390642136,
1108 |       -1.5308352610319678,
1109 |       -1.4255459014764216,
1110 |       -1.336063749518239,
1111 |       -1.2509627812609003,
1112 |       -1.1525346494373787,
1113 |       -1.0713108780905793,
1114 |       -0.97714269079862626,
1115 |       -0.88681637822953985,
1116 |       -0.791126358540986,
1117 |       -0.70547523344251006,
1118 |       -0.599637630944437,
1119 |       -0.48718825178019814,
1120 |       -0.37974760448826295,
1121 |       -0.2795016218666071,
1122 |       -0.17952408496148523,
1123 |       -0.080411292820550526,
1124 |       0.017663495045639403,
1125 |       0.10081547730478847,
1126 |       0.214054640144953,
1127 |       0.30780452190665997,
1128 |       0.38711556229651156,
1129 |       0.47267636408010982,
1130 |       0.57867787175989072,
1131 |       0.67605221754823042,
1132 |       0.7656022369108,
1133 |       0.87455480308332478,
1134 |       0.96714464178414017,
1135 |       1.0550434566309708,
1136 |       1.1567085015876903,
1137 |       1.255540852715912,
1138 |       1.3515016978158554,
1139 |       1.451636875062676,
1140 |       1.5354662011867848,
1141 |       1.6187970556551829,
1142 |       1.7264116502653923,
1143 |       1.8288272904681182,
1144 |       1.9192533955778677,
1145 |       1.992779262315858,
1146 |       2.0665604434541809,
1147 |       2.1654683063944593,
1148 |       2.2588925988145685,
1149 |       2.36971494401558,
1150 |       2.4507160822389626,
1151 |       2.5396132221680809,
1152 |       2.6323745664088003,
1153 |       2.7358796151359335,
1154 |       2.8168781505500435,
1155 |       2.9310086064265333,
1156 |       3.0458480480466896,
1157 |       3.1153793992858483,
1158 |       3.1830916572622479,
1159 |       3.2805278982309853,
1160 |       3.3602057217156682,
1161 |       3.4409111456876653,
1162 |       3.5137337650145763,
1163 |       3.5841554611784443,
1164 |       3.6755220085908982,
1165 |       3.7483352593587473,
1166 |       3.7982815012021756,
1167 |       3.8732848036334779,
1168 |       3.9252775964039861,
1169 |       4.0046445298361482,
1170 |       4.0541106969836553,
1171 |       4.0995555510224007,
1172 |       4.1508702724204172,
1173 |       4.1969521438366071,
1174 |       4.2338943397700248,
1175 |       4.2782121846939072,
1176 |       4.3087064494309439,
1177 |       4.3610961677702162,
1178 |       4.3715668250082693,
1179 |       4.394159369343706,
1180 |       4.4063129455104049,
1181 |       4.4316401302777777,
1182 |       4.4293263774825231,
1183 |       4.4422643716196344,
1184 |       4.4413968612235166,
1185 |       4.4407718054445287,
1186 |       4.4268937435648033,
1187 |       4.4227073806124215,
1188 |       4.4089475668015661,
1189 |       4.3931616288876105,
1190 |       4.3775013857201,
1191 |       4.3617688645342971,
1192 |       4.3416782855555711,
1193 |       4.3243552799096552,
1194 |       4.2944292058986253,
1195 |       4.2582068310526937,
1196 |       4.2118543649760758,
1197 |       4.1598232795435708,
1198 |       4.1138711745775733,
1199 |       4.0590081040637722,
1200 |       4.0012446040502718,
1201 |       3.9277263176821258,
1202 |       3.8445174515296632,
1203 |       3.7825049352748215,
1204 |       3.7107814985712251,
1205 |       3.635358119524799,
1206 |       3.5611187278769538,
1207 |       3.4781852803266347,
1208 |       3.3821987623733443,
1209 |       3.29366047421068,
1210 |       3.2090701824466374,
1211 |       3.1139630025677274,
1212 |       3.0327413076500092,
1213 |       2.9529472227766025,
1214 |       2.8602847855368019,
1215 |       2.7611372232237548,
1216 |       2.674607833884608,
1217 |       2.58279523784013,
1218 |       2.4697532611126389,
1219 |       2.3649371895108757,
1220 |       2.2704433110389295,
1221 |       2.1584698833236229,
1222 |       2.0609908461993776,
1223 |       1.9664097841883481,
1224 |       1.8677776419163163,
1225 |       1.7762558460159314,
1226 |       1.6880140419663965,
1227 |       1.599541206552902,
1228 |       1.5003723552509514,
1229 |       1.4041415101593624,
1230 |       1.3058111050698311,
1231 |       1.2100936733715291,
1232 |       1.0990483618316971,
1233 |       0.99100765977612937,
1234 |       0.88784231472946729,
1235 |       0.7830595953571774,
1236 |       0.68956155360216209,
1237 |       0.59643459537808585,
1238 |       0.4968916835581208,
1239 |       0.39482841871128155,
1240 |       0.30661449247311229,
1241 |       0.21238808826985667,
1242 |       0.11711651492953808,
1243 |       0.0345409029003675,
1244 |       -0.062555502975338739,
1245 |       -0.14044341221748219,
1246 |       -0.23373139943782242,
1247 |       -0.31933767600377927,
1248 |       -0.42039008939791672,
1249 |       -0.52573431609085686,
1250 |       -0.61597823576768185,
1251 |       -0.705258841554615,
1252 |       -0.78350310735883322,
1253 |       -0.87175213799096662,
1254 |       -0.97212874346905287,
1255 |       -1.0503437406156166,
1256 |       -1.1630943482722065,
1257 |       -1.2608320258540671,
1258 |       -1.3533806711737604,
1259 |       -1.4494801475709074,
1260 |       -1.5659131125610719,
1261 |       -1.662661365428838,
1262 |       -1.7464654288098882,
1263 |       -1.8351887075127973,
1264 |       -1.922564263227825,
1265 |       -2.0196937407985991,
1266 |       -2.1087572563211037,
1267 |       -2.2007855082897327,
1268 |       -2.3104031519898163,
1269 |       -2.4010240574580108,
1270 |       -2.4883325469896502,
1271 |       -2.5931139511498706,
1272 |       -2.6852799948844046,
1273 |       -2.7895904654268557,
1274 |       -2.8727518901211382,
1275 |       -2.9628683092486012,
1276 |       -3.0572426870006448,
1277 |       -3.1409039702089481,
1278 |       -3.2311264856956634,
1279 |       -3.3088031586901687,
1280 |       -3.3956344352243972,
1281 |       -3.4923779425460171,
1282 |       -3.5575958890614943,
1283 |       -3.6424595577770678,
1284 |       -3.713895007375315,
1285 |       -3.7858911967561535,
1286 |       -3.8226797003136319,
1287 |       -3.8868904263764295,
1288 |       -3.95495859728524,
1289 |       -4.0108306860493874,
1290 |       -4.0797186930996592,
1291 |       -4.1428707770056112,
1292 |       -4.2208556323770532,
1293 |       -4.2666781553776509,
1294 |       -4.30949137437553,
1295 |       -4.355634931146029,
1296 |       -4.39779255676348,
1297 |       -4.4357554899977032,
1298 |       -4.4671282873768154,
1299 |       -4.483535512652713,
1300 |       -4.5094777442937488,
1301 |       -4.5041518249767112,
1302 |       -4.5168200464672141,
1303 |       -4.5163453856176945,
1304 |       -4.5172222015474839,
1305 |       -4.529140855378742,
1306 |       -4.5236920807412355,
1307 |       -4.502680505315916,
1308 |       -4.504186656801318,
1309 |       -4.5112242204352757,
1310 |       -4.4837993996162959,
1311 |       -4.4700678043809274,
1312 |       -4.4364550302773358,
1313 |       -4.4008778762320082,
1314 |       -4.3759150081265021,
1315 |       -4.3385034779324378,
1316 |       -4.3092907393470981,
1317 |       -4.2671239762116278,
1318 |       -4.1960366273113623,
1319 |       -4.1375496490291246,
1320 |       -4.0822485835599762,
1321 |       -4.0195002826551312,
1322 |       -3.9516661154051422,
1323 |       -3.8919880821151658,
1324 |       -3.8242332560344638,
1325 |       -3.7517721524756111,
1326 |       -3.6775248383008967,
1327 |       -3.6064155204648864,
1328 |       -3.5222181555862555,
1329 |       -3.4435762061833866,
1330 |       -3.3592356749683825,
1331 |       -3.2734120219770211,
1332 |       -3.1774091066285597,
1333 |       -3.1106489587097865,
1334 |       -3.0282245004532844,
1335 |       -2.9365355327261446,
1336 |       -2.8631329674565373,
1337 |       -2.7855083228203834,
1338 |       -2.6898473126978795,
1339 |       -2.5816303369293228,
1340 |       -2.4750712451576415,
1341 |       -2.3798848556955323,
1342 |       -2.2893825850507454,
1343 |       -2.2040995553917266,
1344 |       -2.0961422589619643,
1345 |       -2.0025001552362713,
1346 |       -1.8774397730558945,
1347 |       -1.7753889905813827,
1348 |       -1.6795729488048667,
1349 |       -1.586270036341737,
1350 |       -1.4845199528760169,
1351 |       -1.3784614301207174,
1352 |       -1.2882334531209099,
1353 |       -1.1929454257207128,
1354 |       -1.088662341360271,
1355 |       -0.98941623906208764,
1356 |       -0.87966149289833528,
1357 |       -0.77959952994988979,
1358 |       -0.68409650307311831,
1359 |       -0.60335191184250836,
1360 |       -0.52189273443690232,
1361 |       -0.4081802998248466,
1362 |       -0.31406563883165006,
1363 |       -0.21858509212974911,
1364 |       -0.10862828778820029,
1365 |       -0.010198554117813471,
1366 |       0.09570236658262439,
1367 |       0.20060768180066652,
1368 |       0.28742476449062343,
1369 |       0.37302178146223508,
1370 |       0.45499301034355816,
1371 |       0.53265732025526658,
1372 |       0.64306359228366528,
1373 |       0.74828987264863256,
1374 |       0.8464647215859763,
1375 |       0.9368788420734272,
1376 |       1.0315243943677668,
1377 |       1.1383547269370988,
1378 |       1.2251168632642029,
1379 |       1.3259873870365035,
1380 |       1.4323591495469319,
1381 |       1.5106244513402096,
1382 |       1.6156300331981557,
1383 |       1.7071307683288637,
1384 |       1.8115110289334337,
1385 |       1.8886188827301553,
1386 |       1.9762167125826162,
1387 |       2.0795917308944616,
1388 |       2.1711444776221196,
1389 |       2.261618649914181,
1390 |       2.3573063346469687,
1391 |       2.4697112116055235,
1392 |       2.5598731677591884,
1393 |       2.6595390609995211,
1394 |       2.7514146772943642,
1395 |       2.843746302367312,
1396 |       2.9476908762026306,
1397 |       3.0399303539516049,
1398 |       3.1464668467065309,
1399 |       3.2336730315342752,
1400 |       3.3276084444216036,
1401 |       3.4362578713267453,
1402 |       3.5100396268079859,
1403 |       3.5981288355855749,
1404 |       3.6992958240525811,
1405 |       3.7816186735265109,
1406 |       3.8601586562958676,
1407 |       3.9416109250859104,
1408 |       4.0150454548558532,
1409 |       4.09622465794671,
1410 |       4.1741361342429011,
1411 |       4.22702034848388,
1412 |       4.30702416036104,
1413 |       4.3857676579487279,
1414 |       4.4525445849459162,
1415 |       4.5057463752194895,
1416 |       4.5513281158187295,
1417 |       4.5899056692469991,
1418 |       4.6414501820742053,
1419 |       4.6875783063912744,
1420 |       4.72524999485949,
1421 |       4.7580339551403137,
1422 |       4.7812789307187442,
1423 |       4.7969375081467893,
1424 |       4.8107598232444992,
1425 |       4.8177921688063128,
1426 |       4.824448447006402,
1427 |       4.8348015906153332,
1428 |       4.8393728093018931,
1429 |       4.8391665179043,
1430 |       4.8261908456180596,
1431 |       4.8171673775389516,
1432 |       4.8111695486499535,
1433 |       4.788278683579775,
1434 |       4.7550520783563028,
1435 |       4.7338902161587768,
1436 |       4.70194950919418,
1437 |       4.6816496159300369,
1438 |       4.6180899973224463,
1439 |       4.54295157563113,
1440 |       4.46835081959064,
1441 |       4.4089442137570245,
1442 |       4.3465545885419727,
1443 |       4.2842562426786515,
1444 |       4.2276146534861363,
1445 |       4.1687660132895505,
1446 |       4.0883393611724328,
1447 |       3.9851708353364335,
1448 |       3.8943600624746066,
1449 |       3.8214279000027496,
1450 |       3.7368311135520917,
1451 |       3.6605283218190108,
1452 |       3.6007101790694445,
1453 |       3.4973917141571289,
1454 |       3.404359300201858,
1455 |       3.310684820026943,
1456 |       3.2221267123119182,
1457 |       3.11223689246192,
1458 |       3.0225384702017295,
1459 |       2.9241880526529274,
1460 |       2.844344965853387,
1461 |       2.7645345534171333,
1462 |       2.6731137193123007,
1463 |       2.5948961312438024,
1464 |       2.4679133339971493,
1465 |       2.3695351242016067,
1466 |       2.2735892328594889,
1467 |       2.1899130296571458,
1468 |       2.0676959716841616,
1469 |       1.9629955320995467,
1470 |       1.8608735913154495,
1471 |       1.764264809918662,
1472 |       1.6811369757159755,
1473 |       1.5770789397916278,
1474 |       1.4837514493993018,
1475 |       1.4050647429251675,
1476 |       1.3007416662111932,
1477 |       1.1908446667865316,
1478 |       1.0810665578137344,
1479 |       0.97977259844379527,
1480 |       0.89493762289514778,
1481 |       0.79119710440048674,
1482 |       0.70712672698022938,
1483 |       0.59590404437568867,
1484 |       0.49530966040374,
1485 |       0.40201489445766092,
1486 |       0.30788088466581043,
1487 |       0.21308115669528038,
1488 |       0.11037561777107428,
1489 |       0.035479137348143154,
1490 |       -0.042983152108374514,
1491 |       -0.14543050777097971,
1492 |       -0.2463833217187533,
1493 |       -0.33240985096378867,
1494 |       -0.42227533448388221,
1495 |       -0.5112547821198421,
1496 |       -0.62659479131394835,
1497 |       -0.72657102640752991,
1498 |       -0.83339782858894651,
1499 |       -0.93303167115149976,
1500 |       -1.0570894894489322,
1501 |       -1.1477731819214338,
1502 |       -1.2374540055568335,
1503 |       -1.3381128127801223,
1504 |       -1.4476850271486261,
1505 |       -1.5458238164407447,
1506 |       -1.6583882068663176,
1507 |       -1.7709665775483092
1508 |     ]
1509 |   ],
1510 |   "Y": [
1511 |     0.75764834349419519,
1512 |     0.81632567659518485,
1513 |     0.93501031129298107,
1514 |     1.2501148157788518,
1515 |     0.91992481858028952,
1516 |     0.59463982029121865,
1517 |     0.9087108710446069,
1518 |     1.5559374583855599,
1519 |     0.86225150834790387,
1520 |     0.90073528843895589,
1521 |     1.1347614287583381,
1522 |     1.2843249622836277,
1523 |     1.1757501434062467,
1524 |     0.81773109153789625,
1525 |     0.65444919467408291,
1526 |     0.76524093427525308,
1527 |     0.80250184444173445,
1528 |     1.0573178586736693,
1529 |     1.1060450440817331,
1530 |     1.1546572147711127,
1531 |     0.97431980311818056,
1532 |     1.0925363168442623,
1533 |     0.61682930434502214,
1534 |     0.83756681883104767,
1535 |     0.77257048092799963,
1536 |     1.3831870148332048,
1537 |     0.71661707545278031,
1538 |     0.7924397809871857,
1539 |     1.0746047417980356,
1540 |     0.80008374527890114,
1541 |     0.91905264748614612,
1542 |     0.66340238453842,
1543 |     0.69656853192340551,
1544 |     0.465987011502561,
1545 |     1.0015682931389527,
1546 |     0.95676343414755494,
1547 |     0.914943896904609,
1548 |     0.503444646124521,
1549 |     0.23922809128117734,
1550 |     0.70336784051821144,
1551 |     0.39629442437341345,
1552 |     1.0348988939118189,
1553 |     0.81408500669026118,
1554 |     0.45839991758671478,
1555 |     0.57601887136511409,
1556 |     -0.15659845284110807,
1557 |     0.15274425046666879,
1558 |     0.15838841874761062,
1559 |     0.42996756864620067,
1560 |     0.48304270816563127,
1561 |     0.47483573510362265,
1562 |     0.25594531011151128,
1563 |     -0.22777412720778753,
1564 |     0.1566135723488371,
1565 |     -0.05191408232605077,
1566 |     -0.028822892525310298,
1567 |     0.20325300460110318,
1568 |     -0.082811849744152177,
1569 |     0.10451468203421495,
1570 |     0.081974740198251253,
1571 |     -0.094791968644487162,
1572 |     -0.71731929974451758,
1573 |     0.022549591849728295,
1574 |     -0.59164568614302715,
1575 |     0.44917855467830309,
1576 |     -0.10334828328845744,
1577 |     -0.35035048491461795,
1578 |     -0.071745389986829111,
1579 |     -0.7226643572226199,
1580 |     -0.49244158622698725,
1581 |     -0.27000765992101028,
1582 |     -0.72703544575682555,
1583 |     -0.71018741180313538,
1584 |     -0.36849448522561007,
1585 |     -0.6935149948898689,
1586 |     -0.79893745467145483,
1587 |     -1.5776623556709617,
1588 |     -0.27815485108709126,
1589 |     -0.68244343289142884,
1590 |     -0.74316825125637986,
1591 |     -0.65075645652393077,
1592 |     -0.611547497985304,
1593 |     -0.77759120060907638,
1594 |     -0.75537765606130092,
1595 |     -0.53155340555107133,
1596 |     -1.0164252322124547,
1597 |     -0.9838655739954294,
1598 |     -1.0127040592243006,
1599 |     -0.50615719816247273,
1600 |     -0.95136067952001246,
1601 |     -0.2943034632496957,
1602 |     -1.0118614549661864,
1603 |     -0.91830268936771819,
1604 |     -0.882081267773364,
1605 |     -0.9424278256273122,
1606 |     -1.5449526895706329,
1607 |     -0.80694387349930352,
1608 |     -0.82386692868275746,
1609 |     -1.3249009858077656,
1610 |     -1.3891292757204967,
1611 |     -0.56767823273041207,
1612 |     -1.3466309358184119,
1613 |     -0.94658197612701345,
1614 |     -0.85914534030876233,
1615 |     -1.2036987820177121,
1616 |     -1.0130876704320844,
1617 |     -0.97364631662098622,
1618 |     -0.85876133965433521,
1619 |     -0.64695933187026644,
1620 |     -0.83445841849448,
1621 |     -0.71444842201479686,
1622 |     -1.1483020757313982,
1623 |     -0.90369207822945719,
1624 |     -0.48249730565554771,
1625 |     -1.16141646402308,
1626 |     -0.70689491403155014,
1627 |     -0.55085800693082443,
1628 |     -0.58587913439911632,
1629 |     -0.91332077047145843,
1630 |     -1.2244274579839056,
1631 |     -0.59769195451366586,
1632 |     -0.50091078355056684,
1633 |     -1.4498600790208545,
1634 |     -0.530563782132486,
1635 |     -1.2714987778225966,
1636 |     -1.6794920608540604,
1637 |     -0.77307742673387847,
1638 |     -0.95585738080951488,
1639 |     -0.6624877657420376,
1640 |     -0.41676099374890196,
1641 |     -0.88454033238172614,
1642 |     -1.2680192324177604,
1643 |     -1.0523025827708887,
1644 |     -0.73355564046287691,
1645 |     -1.2435556756124411,
1646 |     -0.611838749346088,
1647 |     -1.4436197397767356,
1648 |     -1.0238481797372105,
1649 |     -0.44087308725320962,
1650 |     -1.0684250132265702,
1651 |     -0.97791640938442881,
1652 |     -0.024602374471384691,
1653 |     -1.4176329544994015,
1654 |     -0.92614346821914018,
1655 |     -0.64548353947497183,
1656 |     -1.3051955307304806,
1657 |     -0.49599440821330237,
1658 |     -0.73984962348761929,
1659 |     -1.1753745749456759,
1660 |     -0.40409713011007281,
1661 |     -0.52175979120059179,
1662 |     -0.81310978650569021,
1663 |     -1.0483641122335083,
1664 |     -0.17450247557986798,
1665 |     -0.33925453846296932,
1666 |     -1.0365964850669442,
1667 |     -0.6060525499911148,
1668 |     -0.88353684814874678,
1669 |     -0.95448475499333907,
1670 |     -0.70549489187232239,
1671 |     0.03385502101657345,
1672 |     -0.12422726146986063,
1673 |     -0.79308969445443023,
1674 |     -0.48397232455634415,
1675 |     -0.78688922975230913,
1676 |     -0.3943762617312917,
1677 |     -0.53042147113116289,
1678 |     -0.37719011221679211,
1679 |     -0.2951799700092,
1680 |     -0.20124008543953351,
1681 |     -0.44818573850098992,
1682 |     0.29426371849129318,
1683 |     -0.012909703086409569,
1684 |     -0.071894057296789771,
1685 |     -0.28740285765638496,
1686 |     -0.078002736606456388,
1687 |     -0.25056819002427361,
1688 |     0.32349868862557113,
1689 |     0.0915749263991194,
1690 |     -0.077347743811773922,
1691 |     0.43310962401904074,
1692 |     -0.49504043049766389,
1693 |     -0.69319385131272027,
1694 |     0.24566796930207513,
1695 |     0.099296194342869848,
1696 |     0.51978469850144937,
1697 |     0.21983880449982798,
1698 |     0.35721884588862229,
1699 |     0.66837669305370329,
1700 |     0.46063475989512487,
1701 |     0.15402889685982657,
1702 |     0.69216637715038276,
1703 |     0.341881149822677,
1704 |     0.5089106750431498,
1705 |     0.894441876840068,
1706 |     0.3995510087173833,
1707 |     0.95508583435332883,
1708 |     1.0904217538278704,
1709 |     0.85578019481919154,
1710 |     1.4407412382434577,
1711 |     0.763678549127937,
1712 |     1.036051888724969,
1713 |     1.2751276085529388,
1714 |     1.339272926894282,
1715 |     0.82440609306307366,
1716 |     0.7867819469617785,
1717 |     1.3520996509396082,
1718 |     0.80634561296671181,
1719 |     0.78158841142428248,
1720 |     0.71793537231969418,
1721 |     0.9672723250358185,
1722 |     0.35186316144366969,
1723 |     1.6240191296544504,
1724 |     0.22505329717382005,
1725 |     1.0108516705120372,
1726 |     1.03184588804622,
1727 |     1.2982584623311695,
1728 |     0.1839901036775502,
1729 |     0.88194202121399767,
1730 |     1.9650881863856471,
1731 |     1.3216387469069142,
1732 |     1.3619704140842419,
1733 |     1.3026797993903754,
1734 |     1.077939729142847,
1735 |     1.493744451515719,
1736 |     0.77094762128984451,
1737 |     1.0268908007672306,
1738 |     0.76421868162191953,
1739 |     0.96021154654360885,
1740 |     0.91992576287325778,
1741 |     0.78524604136302079,
1742 |     1.1543965780663561,
1743 |     1.2649118679917626,
1744 |     0.80519448932621274,
1745 |     1.1582742911178783,
1746 |     0.99776404565657717,
1747 |     1.1013531406493726,
1748 |     0.83814082721082184,
1749 |     1.2101569017924581,
1750 |     1.1511404799459202,
1751 |     1.5067440520380244,
1752 |     1.6633848820041286,
1753 |     0.63650206628409789,
1754 |     0.47446866174380908,
1755 |     0.99416557336541311,
1756 |     1.0766740315773329,
1757 |     0.51375643089418888,
1758 |     0.58706652522068814,
1759 |     1.1225563688088669,
1760 |     1.4660315626781473,
1761 |     0.9462462428206,
1762 |     0.86112056652711777,
1763 |     0.618565280637805,
1764 |     0.89475481720735073,
1765 |     0.90149918348089253,
1766 |     0.7776742323065382,
1767 |     0.83111572454275784,
1768 |     0.82287003038570394,
1769 |     1.0474266006379565,
1770 |     1.5152476344001522,
1771 |     1.3835922765898876,
1772 |     0.763733128747909,
1773 |     0.86017682800387762,
1774 |     1.2815349341476225,
1775 |     0.420884103970228,
1776 |     1.0645664488687199,
1777 |     0.84657477423931471,
1778 |     0.838387003649429,
1779 |     0.6021039544855179,
1780 |     0.999647488212171,
1781 |     0.38874198382648173,
1782 |     0.21334544771389263,
1783 |     0.83715398704511435,
1784 |     0.47545482964176211,
1785 |     0.72988399164771034,
1786 |     0.78878666615114545,
1787 |     0.99476567969633722,
1788 |     0.72492515449441286,
1789 |     0.72470473451124973,
1790 |     0.62517047759879574,
1791 |     0.91477279093268182,
1792 |     0.94672360942768186,
1793 |     0.05513959106248123,
1794 |     0.68405348003201294,
1795 |     0.0043717972996809573,
1796 |     0.42369662271143149,
1797 |     0.43008466977317256,
1798 |     0.43764024782734,
1799 |     0.23657493163258908,
1800 |     0.742691991920529,
1801 |     0.36666641598248212,
1802 |     0.0066520112956943434,
1803 |     0.013073844280628161,
1804 |     -0.50403396055018379,
1805 |     0.33510308959312857,
1806 |     0.22851301359539483,
1807 |     0.03272114056110989,
1808 |     -0.3636305759597121,
1809 |     -0.13289140199766983,
1810 |     -0.13804026398800526,
1811 |     -0.2886846308970934,
1812 |     -0.20735362013628417,
1813 |     -0.15774463620096849,
1814 |     -0.88667441541078418,
1815 |     -0.5306684005461817,
1816 |     -0.72037757697616911,
1817 |     -0.62917532142718924,
1818 |     -0.77007524155736262,
1819 |     -0.30169513147512905,
1820 |     -0.49271156464696725,
1821 |     -0.077522719432617127,
1822 |     -1.1481134730945624,
1823 |     -0.28734466831643618,
1824 |     -0.39074129102247729,
1825 |     -0.87323539779582993,
1826 |     -0.68873376549299736,
1827 |     -1.531096699399169,
1828 |     -0.95135985169641213,
1829 |     -0.76512636215216112,
1830 |     -0.25873943352693918,
1831 |     -0.80710452369302244,
1832 |     -0.24914005051794619,
1833 |     -1.2046826076294914,
1834 |     -0.71973258598735645,
1835 |     -0.93837799394814225,
1836 |     -1.0616393963781192,
1837 |     -0.81261169029572933,
1838 |     -0.91279126154263646,
1839 |     -1.2974962519481161,
1840 |     -0.86284485080071693,
1841 |     -1.0424577814571492,
1842 |     -1.4073688053055364,
1843 |     -0.75394891272615383,
1844 |     -1.2458928874424526,
1845 |     -0.71743280722395875,
1846 |     -1.6862542770529596,
1847 |     -1.3715297435650664,
1848 |     -0.1678391362136411,
1849 |     -0.42025391767681797,
1850 |     -1.1096176836951579,
1851 |     -0.86918072667032142,
1852 |     -0.77837275534397987,
1853 |     -1.2320541961724172,
1854 |     -1.4541121119774292,
1855 |     -0.85590456191119157,
1856 |     -1.3649802038603043,
1857 |     -1.4308908718376596,
1858 |     -0.63448871929329387,
1859 |     -0.91248220308416084,
1860 |     -0.77863044757849331,
1861 |     -0.94371087806330178,
1862 |     -0.87554070526131778,
1863 |     -1.3128477097513249,
1864 |     -0.690586478726152,
1865 |     -0.88699662608444241,
1866 |     -0.65363352044755185,
1867 |     -1.3443411645664354,
1868 |     -1.1479532400429857,
1869 |     -0.41574191641580294,
1870 |     -0.69562643231189591,
1871 |     -0.90685555168162468,
1872 |     -0.82151871873530879,
1873 |     -0.66020822585407368,
1874 |     -1.8058325706047034,
1875 |     -1.207280290610808,
1876 |     -1.0518378712088396,
1877 |     -1.3205368531674664,
1878 |     -0.83769590406763039,
1879 |     -0.30107456000412869,
1880 |     -0.80555395893628723,
1881 |     -1.0604638269359736,
1882 |     -0.89028484718437029,
1883 |     -0.92391738896867659,
1884 |     -0.6511438001617571,
1885 |     -1.357112161282833,
1886 |     -0.639169296795838,
1887 |     -1.0203566127919887,
1888 |     -1.18116857586094,
1889 |     -0.87910278682813625,
1890 |     -1.2368377131631354,
1891 |     -1.234362308301564,
1892 |     -0.68195353485528343,
1893 |     -1.0736270679766007,
1894 |     -0.86807625597987659,
1895 |     -0.82781581685805372,
1896 |     -0.37122411995797766,
1897 |     -0.404120625563109,
1898 |     -1.3559311725568812,
1899 |     -1.096616435480068,
1900 |     -0.80757838940513271,
1901 |     -0.16905323272066219,
1902 |     -0.956691294952042,
1903 |     -1.0351925485781757,
1904 |     -1.0530785854995859,
1905 |     -1.5528780972619582,
1906 |     -0.14923810641921464,
1907 |     -0.88668435585479788,
1908 |     -0.67380598356315868,
1909 |     -0.90812115413068473,
1910 |     -0.42196155337220526,
1911 |     -0.833821035299807,
1912 |     -1.1584571558981678,
1913 |     -0.79044364947593637,
1914 |     -0.44247253716986223,
1915 |     -0.63690464800949009,
1916 |     -0.67782242919407043,
1917 |     -0.70216012330328326,
1918 |     -0.13295599329831581,
1919 |     -0.27406169670534453,
1920 |     -0.038561799038976385,
1921 |     -0.5977923355114928,
1922 |     -0.52666603769216924,
1923 |     -0.34014081359195003,
1924 |     -0.084095665868092334,
1925 |     -0.57055967182877321,
1926 |     -0.080437767276559324,
1927 |     0.19303594216097697,
1928 |     -0.20923979872541829,
1929 |     0.18743897940282453,
1930 |     0.27295915786338359,
1931 |     0.0607373748264872,
1932 |     -0.041205504701913975,
1933 |     0.28013105122727855,
1934 |     0.028652291260623408,
1935 |     0.14518274848054186,
1936 |     0.030166120320819334,
1937 |     0.059588339760043962,
1938 |     0.48730148971392295,
1939 |     0.6153078414174018,
1940 |     0.087690416089786227,
1941 |     0.67321211811383752,
1942 |     0.5656941010569434,
1943 |     0.63685091183969944,
1944 |     0.67689505834207109,
1945 |     0.83527292849939583,
1946 |     0.562074856898948,
1947 |     0.84652681946511521,
1948 |     0.91316918405642689,
1949 |     0.796092186507785,
1950 |     1.139365045905067,
1951 |     1.0991283809894699,
1952 |     0.30089134881674662,
1953 |     1.3032257225255586,
1954 |     1.3333701164462155,
1955 |     0.06887825450644891,
1956 |     1.232186153650324,
1957 |     1.2479624927624917,
1958 |     0.80416678671152231,
1959 |     1.2620153468214825,
1960 |     1.5336673751323722,
1961 |     1.3987245628756759,
1962 |     1.608480818410061,
1963 |     1.3717726900865557,
1964 |     0.88845045634464781,
1965 |     1.1264983431634257,
1966 |     0.81501191506691506,
1967 |     1.0333493555036277,
1968 |     1.4676076862554228,
1969 |     1.6330468325575618,
1970 |     0.66895531599346869,
1971 |     0.61157487855300841,
1972 |     1.5285756201926266,
1973 |     1.0005310650755608,
1974 |     1.2304773830160531,
1975 |     1.5380888362419012,
1976 |     0.350449859898303,
1977 |     0.79266256368992838,
1978 |     1.4971361781091634,
1979 |     1.0944670372181031,
1980 |     0.6641995413642573,
1981 |     0.77492491561559684,
1982 |     1.0454162577718933,
1983 |     1.1016376731156321,
1984 |     1.1678238784679804,
1985 |     0.79723690758656107,
1986 |     0.049966369297049118,
1987 |     0.78901295133751792,
1988 |     0.78763258053987428,
1989 |     1.1631946264828048,
1990 |     0.8509548600923541,
1991 |     1.2451811650788309,
1992 |     0.51792337236044772,
1993 |     0.77930072777102255,
1994 |     0.5716574631104463,
1995 |     1.2905438838361887,
1996 |     1.2571201052030814,
1997 |     0.75611008159395,
1998 |     0.86204697637773375,
1999 |     0.95392955283693182,
2000 |     0.73626994756425634,
2001 |     1.2032498719317308,
2002 |     0.67820371504497556,
2003 |     1.4030307890278866,
2004 |     1.4838011137318512,
2005 |     0.93645999768775368,
2006 |     0.75522092164496513,
2007 |     1.1382503870469791,
2008 |     1.1066152541233427,
2009 |     0.95403948911638892,
2010 |     1.2851477530543052
2011 |   ]
2012 | }


--------------------------------------------------------------------------------
/python/exercises/README.md:
--------------------------------------------------------------------------------
1 | # Exercises
2 | 
3 | Resources and template codes for Bayesian Filtering and Smoothing book exercises.


--------------------------------------------------------------------------------
/python/exercises/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/EEA-sensors/Bayesian-Filtering-and-Smoothing/efc88d0de842ee6522c56d342fadf8876a60d7f5/python/exercises/__init__.py


--------------------------------------------------------------------------------
/python/requirements.txt:
--------------------------------------------------------------------------------
1 | matplotlib==3.2.*
2 | numpy==1.19.*
3 | scipy==1.5.*


--------------------------------------------------------------------------------
/python/setup.py:
--------------------------------------------------------------------------------
 1 | from setuptools import setup, find_packages
 2 | 
 3 | with open("../README.md", "r") as fh:
 4 |     long_description = fh.read()
 5 | 
 6 | setup(
 7 |     name="filtering-and-smoothing",
 8 |     version="1.0.0",
 9 |     description="Companion Code for Bayesian Filtering and Smoothing book",
10 |     long_description=long_description,
11 |     long_description_content_type="text/markdown",
12 |     url="https://github.com/EEA-sensors/Bayesian-Filtering-and-Smoothing",
13 |     packages=find_packages(),
14 |     classifiers=[
15 |         "Programming Language :: Python :: 3",
16 |         "License :: OSI Approved :: MIT License",
17 |         "Operating System :: OS Independent",
18 |     ],
19 |     install_requires=["matplotlib>=3.2",
20 |                       "numpy>=1.19",
21 |                       "scipy>=1.5"],
22 |     python_requires='>=3.6',
23 | )


--------------------------------------------------------------------------------