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.