<SYSTEM_TASK:>
Solve the following problem using Python, implementing the functions described below, one line at a time
<END_TASK>
<USER_TASK:>
Description:
 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)