algorithm statistics artificial-intelligence hidden-markov-models

HMM Localization in 2D maze, trouble applying smoothing (backward algorithm)

We use HMM (Hidden Markov Model) to localize a robot in a windy maze with damaged sensors. If he attempts to move in a direction, he will do so with a high probability, and a low chance to accidentally go to either side. If his movement would make him go over an obstacle, he will bounce back to the original tile.

From any given position, he can sense in all four directions. He will notice an obstacle if it is there with high certainty, and see an obstacle when there is none with low certainty.

We have a probability map for all possible places the robot might be in the maze, since he knows what the maze looks like. Initially it all starts evenly distributed.

I have completed the motion and sensing aspect of this and am getting the proper answers, but I am stuck on smoothing (backward algorithm).

Assume that the robot performs the following sequence of actions: senses, moves, senses, moves, senses. This gives us 3 states in our HMM model. Assume that the results I have at each step of the way so far are correct.

I am having a lot of trouble performing smoothing (backward algorithm), given that there are four conditional probabilities (one for each direction).

Assume SP is for smoothing probability, BP is for backward probability

Assume Sk is for a state, and Zk is for an observation at that state. The problem for me is figuring out how to construct my backwards equation given that each Zk is only for a single direction.

I know the algorithm for smoothing is: SP(k) is proportional to BP(k+1) * P(Sk | Z1:k)

Where BP(k+1) is defined as :

if (k == n) return 1 else return Sum(s) of BP(k+1) * P(Zk+1|Sk+1) * P(Sk+1=s | Sk)

This is where I am having my trouble. Mainly in the Conditional Probability portion of this equation. Because each spot has four different directions that it observed! In other words, each state has four different evidence variables as opposed to just one! Do I average these values? Do I do a separate summation for them? How do I account for multiple observations at a given state and properly condense it into this equation which only has room for one conditional probability?

Here is the code I have performing the smoothing:

public static void Smoothing(List<int[]> observations) {

        int n = observations.Count; //n is Total length of evidence sequence
        int k = n - 1;              //k is the state we are trying to smooth. start with n-1

        for (; k >= 1; k--) {       //Smooth all the way back to the first state
            for (int dir = 0; dir < 4; dir++) {

                //We must smooth each direction separately
                SmoothDirection(dir, observations, k, n);
            }

            Console.WriteLine($"Smoothing for k = {k}\n");
            UpdateMapMotion(mapHistory[k]);
            PrintMap();
        }
    }

    public static void SmoothDirection(int dir, List<int[]> observations, int k, int n) {
        var alphas = new double[ROWS, COLS];
        var normalizer = 0.0;
        int row, col;
        foreach (var t in map) {
            if (t.isObstacle) continue;
            row = t.pos.y;
            col = t.pos.x;
            alphas[row, col] = mapHistory[k][row, col] 
                * Backwards(k, n, t, dir, observations, moves[^(n - k)]);
            normalizer += alphas[row, col];
        }

        UpdateHistory(k, alphas, normalizer);
    }

    public static void UpdateHistory(int index, double[,] alphas, double normalizer) {
        for (int r = 0; r < ROWS; r++) {
            for (int c = 0; c < COLS; c++) {
                mapHistory[index][r, c] = alphas[r, c] / normalizer;
            }
        }
    }

    public static double Backwards(int k, int n, Tile t, int dir, List<int[]> observations, int moveDir) {
        if (k == n) return 1;
        double p = 0;
        var nextStates = GetPossibleNextStates(t, moveDir);
        foreach (var s in nextStates) {
            p += Cond_Prob(s.hasObstacle[dir], observations[^(n - k)][dir] == 1) * Trans_Prob(t, s, moveDir) 
                * Backwards(k+1, n, s, dir, observations, moves[^(n - k)]); 
        }
        return p;
    }

    public static List<Tile> GetPossibleNextStates(Tile t, int direction) {
        var tiles = new List<Tile>();                   //Next States
        var perpDirs = GetPerpendicularDir(direction);  //Perpendicular Directions

        //If obstacle in front of Tile t or on the sides, Tile t is a possible next state.
        if (t.hasObstacle[direction] || t.hasObstacle[perpDirs[0]] || t.hasObstacle[perpDirs[1]])
            tiles.Add(t);

        //If there is no obstacle in front of Tile t, then that tile is a possible next state.
        if (!t.hasObstacle[direction])
            tiles.Add(GetTileAtPos(t.pos + directions[direction]));

        //If there are no obstacles on the sides of Tile t, then those are possible next states.
        foreach (var dir in perpDirs) {
            if (!t.hasObstacle[dir])
                tiles.Add(GetTileAtPos(t.pos + directions[dir]));
        }

        return tiles;
    }

TL;DR : How do I perform smoothing (backward algorithm) in a Hidden Markov Model when there are 4 evidences at each state as opposed to just 1?

Solution

SOLVED!

It was actually rather much more simple than I imagined.

I don't actually need to each iteration separately in each direction.

I just need to replace the Cond_Prob() function with Joint_Cond_Prob() which finds the joint probability of all directional observations at a given state.