# Finite grid approximation to the Bayesian filtering problem

I need some hints for solving Ecercise 4.4 from Bayesian Filtering & Smoothing by Simo Särkkä:

Select a finite interval in the state space, say, $$x \in [-10, 10]$$ and discretize it evenly to N subintervals (e.g. N = 1000). Using a suitable numerical approximation to the integrals in the Bayesian filtering equations, implement a finite grid approximation to the Bayesian filter for the Gaussian random walk in Example 4.1. Verify that the result is practically the same as that of the Kalman filter above.

The solution I envisage is:

• start from an initial distribution for the hidden state over the intervals;

• propagate that discrete distribution and update it such that at each step it incorporates new information.

In a way, just as a particle filter without resampling, only with weight updating, right?

My understanding is that I do not need to use the Kalman filter equations, all I am concerned with is the Bayesian filtering equations of section 4.2.

Here is my code:

import numpy as np
from scipy import stats
import plotly.graph_objects as go

N = 100
xs = np.arange(N)
np.random.seed(3)

# Simulate Random Walk
X = np.zeros(N)
for i in range(1, N):
X[i] = X[i-1] + np.random.normal()
Y = X + np.random.normal(size=N)

# Discretization
x = np.linspace(-20,20,1001)
_x_ = 0.5*(x[1:]+x[:-1])        # use mid-interval values

# Initial weight and assumed variance
w = np.full(len(_x_), fill_value=1/len(_x_))
P = 2

MM = np.zeros(len(Y))
for i in range(len(Y)):
CDF = stats.norm.cdf(x, loc=Y[i], scale=np.sqrt(P+1))
w *= np.diff(CDF)  # update distribution
w /= np.sum(w)     # normalize
#     w += 1e-16         # avoid degeneracy

m = w @ _x_
P = w @ ((_x_-m)**2)

MM[i] = m

fig = go.Figure()
fig.layout.update(height=600)
fig.show()


The resulting plot is:

This is clearly not "practically the same as that of the Kalman filter" (provided solution on page 59).

If I uncomment the line w += 1e-16 to bump slightly the weights so that they don't degenerate, I get this plot:

Slightly better, but still, not quite the same.

I tried to implement both Bayes and Kalman filtering in Matlab and got the same result.

In your code I could not find the prediction step. Without prediction the filter 'converges' too fast to a wrong value and could not accept new measurements because of the very small state variance.

While developing the code it was useful for me to plot both prior and posterior to make sure the calculation is correct. Maybe it can help you to debug your code.

I had some difficulties with the Bayes prediction step, so I implemented it quick and dirty by recalculating the grid state using mu and sigma values. May be you have a better solution for it.

function [] = fusion()
n = 1000;
a = -20;
b = 20;

N = 100;
RefX = zeros(N, 1);

BayesFusion = zeros(N, 1);
KalmanFusion = zeros(N, 1);

Q = 1;
R = 1;

% initial covariance matrix
P0 = 10;

% transition matrix for Kalman
F = 1;

% observation matrix for Kalman
H = 1;

% random walk generator
for i=2:N
RefX(i) = RefX(i-1) + randn()*sqrt(Q);
end

% measurements
Y = RefX + randn(size(RefX))*sqrt(R);

% Grid Map for the Bayes filter
gridMap = (linspace(a, b-(b-a)/n, n))';
dx = gridMap(2) - gridMap(1);

for i=1:N

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%      BAYES FILTERING                                 &
if (i == 1)
BayesX = initBayes(gridMap, Y(i), P0);
else
BayesX = predictBayes(gridMap, BayesX, Q);

likelihood = mapValueToGrid(gridMap, Y(i), R);
BayesX = updateBayes(likelihood, BayesX, dx);
end

BayesFusion(i) = getGaussianParams(gridMap, BayesX);

%      BAYES FILTERING                                 &
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%      KALMAN FILTERING                                &
if (i == 1)
[KalmanX, P] = initKalman(Y(i), P0); % initialize the state using the 1st measurement
else
[KalmanX, P] = predictKalman(KalmanX, P, Q, F); %Prediction

[KalmanX, P] = updateKalman(KalmanX, P, Y(i), R, H); %Update
end

KalmanFusion(i) = KalmanX;

%      KALMAN FILTERING                                &
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end

figure;
plot(RefX, 'LineWidth', 2);
hold on;
plot(Y, '.', 'MarkerSize', 12);
plot(BayesFusion, 'LineWidth', 2);
plot(KalmanFusion, 'LineWidth', 2);
hold off;
grid minor;
legend('True State', 'Measurements', 'Bayes Filter', 'Kalman Filter');

end

function [X] = initBayes(gridMap, mu, var)
X = mapValueToGrid(gridMap, mu, var);
end

function posterior = updateBayes(likelihood, prior, dx)
posterior = likelihood.*prior;
posterior = posterior/(sum(posterior)*dx);
end

function prior = predictBayes(gridMap, posterior, Q)
%quick and dirty
%apply the Q value to make the distribution broader

[mu, var] = getGaussianParams(gridMap, posterior);
prior = mapValueToGrid(gridMap, mu, var + Q);
end

function [mu, var] = getGaussianParams(gridMap, pdf)
dx = gridMap(2)-gridMap(1);
mu = gridMap'*pdf*dx;

var = sum(((gridMap - mu).^2).*pdf)*dx;
end

function [mapedValue] = mapValueToGrid(gridMap, mu, var)
var2 = 2*var;
mapedValue = exp(-(gridMap - mu).^2 / var2) / sqrt(pi*var2);
end

function [X, P] = initKalman(Y, P0)
X = Y;
P = P0;
end

function [X, P] = predictKalman(X, P, Q, F)
X = F*X;
P = F*P*F' + Q;
end

function [X, P] = updateKalman(X, P, y, R, H)
Inn = y - H*X;
S = H*P*H' + R;
K = P*H'/S;

X = X + K*Inn;
P = P - K*H*P;
end


• Looks good to me. But, I think there should be a sqrt(var + Q) in the predictBayes function instead of sigma + sigmaQ? – Sandu Ursu Oct 24 '19 at 16:09
• Sure, you are right! I'll correct the function. Thanks for the review. – Anton Oct 24 '19 at 16:35