I have implemented A* search in MATLAB, but I am looking for ways to increase the speed and optimize it. I have tried using a priority queue but I found it doesn't work that well, so I am using a different way to implement the search. I will explain the details, so it might get a bit long. I appreciate your patience.

The grid that I am performing the search on is called `workSpace`. I am using the cell indexes, so by checking `workSpace[inx] == 0` I can tell if the cell is occupied or not, `0 -> free` and `1 -> occupied`. This is the main body of the A*. I am passing the work space, the index for the start cell, and the index of the goal cell. As well as the heauristic `h`, and cost `g` functions. `nNodes` is the total number of nodes, which I use to find the successor nodes.

    function [visitedNodes, f, cameFrom] = aStar(workSpace, startIndx, goalIndx, nNodes, h, g)

        dim = sqrt(nNodes);
        node = startIndx;
        
        cameFrom(nNodes, 1) = 0;
        cameFrom(node) = node;

        closedSet(nNodes, 1) = 0;
        openSet(nNodes, 1) = 0;
        
        costSoFar(nNodes, 1) = 0;       
        
        f = inf(nNodes, 1);

        openSet(node) = 1;
        costSoFar(node) = 0;
        f(node) = 0;

        visitedNodes = 0;

        while sum(openSet) ~= 0
            [~, minFIndx] = min(f);
            f(minFIndx) = inf;
            currentNode = minFIndx;

            if currentNode == goalIndx
                disp('goal Found')
                return
            end

            openSet(currentNode) = 0;
            closedSet(currentNode) = 1;

            childNodes = search.getNeighboursByIndx(workSpace, currentNode, nNodes, dim);

            for i = 1:numel(childNodes)
                if closedSet(childNodes(i)) == 1
                    continue
                end

                tentativeGScore = costSoFar(currentNode) + g(currentNode);

                if openSet(childNodes(i)) ~= 1 || tentativeGScore < costSoFar(childNodes(i))     
                    cameFrom(childNodes(i)) = currentNode;
                    costSoFar(childNodes(i)) = tentativeGScore;
                    f(childNodes(i)) = costSoFar(childNodes(i)) + h(childNodes(i));
                    if openSet(childNodes(i)) == 0
                        openSet(childNodes(i)) = 1;                    
                    end
                end
            end
        end
    end

As I mentioned, I am not using a priority queue. I am using the below mechanism to simulate the priority queue. 

        [~, minFIndx] = min(f);
        f(minFIndx) = inf;
        currentNode = minFIndx;

`min` searches through `f`, which is `f = g+h`, and returns the index of the lowest cell and then I set the value of that cell to `inf` so it doesn't come up again in the next round. I use the below function to get the successors, it is also very simple:

    function successors = getNeighboursByIndx(workSpace, nodeIndx, nNodes, dim)
        delta = [ 1;  dim;... 
                 -1; -dim];
             
        neighbours = bsxfun(@plus, delta, nodeIndx);
        
        % Create the successor matrix and check if all neighbours are within the grid/freeSpace
        % if not, don't add them to the successors matrix
        successors(4, 1) = 0;

        for i=1:4        
            % (1) the index can't be negative 
            % (2) the index should be smaller than the total number of nodes in the grid
            % (3) the index should not be on the wall around the working space
            %           (1)                         (2)                         (3)
            if (neighbours(i) > 0) && (neighbours(i) <= nNodes) && (mod(neighbours(i), dim) ~= 1) 
                if ~(workSpace(neighbours(i)) == 1) % if the index is not in the wallSpace(1) it is in the freeSpace(0)
                    successors(i) = neighbours(i);
                end
            end
        end

        % remove the neighbours that are not eligible as a successor
        % again, `successors` contains the indexes of neighbouring cells
        successors(successors == 0) = [];
    end

This function is very simple. I use only 4-neighbours, Up(1)-Down(-1) | Right(dim)-Left(-dim).

The current algorithm completes the search on 1681 cells in around `0.05 seconds`. The profiler is telling me that the `getNeighboursByIndx` function takes almost 50% of the total time. In this specific work space it gets called 411 times. Please let me know if it is not clear and you need more information.