'The B operator, mapping P into
.. math::
B(P) := R - \beta^2 A\'PB(Q + \beta B\'PB)^{-1}B\'PA + \beta A\'PA
and also returning
.. math::
F := (Q + \beta B\'PB)^{-1} \beta B\'PA
Parameters
P : array_like(float, ndim=2)
A matrix that should be n x n
Returns
F : array_like(float, ndim=2)
The F matrix as defined above
new_p : array_like(float, ndim=2)
The matrix P after applying the B operator'

def b_operator(self, P):

    (A, B, Q, R, beta) = (self.A, self.B, self.Q, self.R, self.beta)
    S1 = (Q + (beta * dot(B.T, dot(P, B))))
    S2 = (beta * dot(B.T, dot(P, A)))
    S3 = (beta * dot(A.T, dot(P, A)))
    F = solve(S1, S2)
    new_P = ((R - dot(S2.T, solve(S1, S2))) + S3)
    return (F, new_P)
