Veryfy that on exit the matrix has appropriate shape for the KF algorithm.

        Input:
            p_M: matrix
                As it is given for the user
            state_dim: int
                State dimensioanlity
            measurement_dim: int
                Measurement dimensionality
            which: string
                One of: 'A', 'Q', 'H', 'R'
        Output:
        ---------------
            p_M: matrix of the right shape

            old_M_shape: tuple
                Old Shape

def _check_SS_matrix(p_M, state_dim, measurement_dim, which='A'):
        """
        Veryfy that on exit the matrix has appropriate shape for the KF algorithm.

        Input:
            p_M: matrix
                As it is given for the user
            state_dim: int
                State dimensioanlity
            measurement_dim: int
                Measurement dimensionality
            which: string
                One of: 'A', 'Q', 'H', 'R'
        Output:
        ---------------
            p_M: matrix of the right shape

            old_M_shape: tuple
                Old Shape
        """

        old_M_shape = None
        if len(p_M.shape) < 3: # new shape is 3 dimensional
            old_M_shape = p_M.shape # save shape to restore it on exit
            if len(p_M.shape) == 2: # matrix
                p_M.shape = (p_M.shape[0],p_M.shape[1],1)
            elif len(p_M.shape) == 1: # scalar but in array already
                if (p_M.shape[0] != 1):
                    raise ValueError("Matrix %s is an 1D array, while it must be a matrix or scalar", which)
                else:
                    p_M.shape = (1,1,1)

        if (which == 'A') or (which == 'Q'):
            if (p_M.shape[0] != state_dim) or (p_M.shape[1] != state_dim):
                raise ValueError("%s must be a square matrix of size (%i,%i)" % (which, state_dim, state_dim))
        if (which == 'H'):
            if (p_M.shape[0] != measurement_dim) or (p_M.shape[1] != state_dim):
                raise ValueError("H must be of shape (measurement_dim, state_dim) (%i,%i)" % (measurement_dim, state_dim))
        if (which == 'R'):
            if (p_M.shape[0] != measurement_dim) or (p_M.shape[1] != measurement_dim):
                raise ValueError("R must be of shape (measurement_dim, measurement_dim) (%i,%i)" % (measurement_dim, measurement_dim))

        return (p_M,old_M_shape)