Balances State-Space model for more numerical stability

    This is based on the following:

     dx/dt = F x + L w
         y = H x

     Let T z = x, which gives

     dz/dt = inv(T) F T z + inv(T) L w
         y = H T z

def balance_ss_model(F,L,Qc,H,Pinf,P0,dF=None,dQc=None,dPinf=None,dP0=None):
    """
    Balances State-Space model for more numerical stability

    This is based on the following:

     dx/dt = F x + L w
         y = H x

     Let T z = x, which gives

     dz/dt = inv(T) F T z + inv(T) L w
         y = H T z
    """

    bF,T,T_inv = balance_matrix(F)

    bL = np.dot( T_inv, L)
    bQc = Qc # not affected

    bH = np.dot(H, T)

    bPinf = np.dot(T_inv, np.dot(Pinf, T_inv.T))

    #import pdb; pdb.set_trace()
#    LL,islower = linalg.cho_factor(Pinf)
#    inds = np.triu_indices(Pinf.shape[0],k=1)
#    LL[inds] = 0.0
#    bLL = np.dot(T_inv, LL)
#    bPinf = np.dot( bLL, bLL.T)

    bP0 = np.dot(T_inv, np.dot(P0, T_inv.T))

    if dF is not None:
        bdF = np.zeros(dF.shape)
        for i in range(dF.shape[2]):
            bdF[:,:,i] = np.dot( T_inv, np.dot( dF[:,:,i], T))

    else:
        bdF = None

    if dPinf is not None:
        bdPinf = np.zeros(dPinf.shape)
        for i in range(dPinf.shape[2]):
            bdPinf[:,:,i] = np.dot( T_inv, np.dot( dPinf[:,:,i], T_inv.T))

#            LL,islower = linalg.cho_factor(dPinf[:,:,i])
#            inds = np.triu_indices(dPinf[:,:,i].shape[0],k=1)
#            LL[inds] = 0.0
#            bLL = np.dot(T_inv, LL)
#            bdPinf[:,:,i] = np.dot( bLL, bLL.T)


    else:
        bdPinf = None

    if dP0 is not None:
        bdP0 = np.zeros(dP0.shape)
        for i in range(dP0.shape[2]):
            bdP0[:,:,i] = np.dot( T_inv, np.dot( dP0[:,:,i], T_inv.T))
    else:
        bdP0 = None


    bdQc = dQc # not affected

    # (F,L,Qc,H,Pinf,P0,dF,dQc,dPinf,dP0)

    return bF, bL, bQc, bH, bPinf, bP0, bdF, bdQc, bdPinf, bdP0