0
$\begingroup$

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.add_scatter(x=xs, y=X, name='Hidden State')
fig.add_scatter(x=xs, y=Y, mode='markers', name='Measurements')
fig.add_scatter(x=xs, y=MM, name='Inferred')
fig.layout.update(height=600)
fig.show()

The resulting plot is:

enter image description here

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: enter image description here

Slightly better, but still, not quite the same.

$\endgroup$
1
$\begingroup$

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

Bayes and Kalman Filtering for the Random Walk process

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

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.