Full LSQ, needs lots of testing