For 3x3 matrices this should be easy. Here's a least-squares routine in Fortran that forms Moore-Penrose pseudoinverse in the process of solution, it might help:
function solve_leastsq(A,b,m,n) result(x)
!
! Solve system with m x n system matrix in least square sense (minimizing Euclidean norm).
! System is overdetermined so we solve A'A * x = A'b, where A' is transpose of A.
! The solution is given by x = (A'A)^(-1)A'*b
!
! Result
real(dp), dimension(n) :: x
! Input
real(dp), dimension(m,n), intent(in) :: A
real(dp), dimension(m), intent(in) :: b
! Locals
real(dp), dimension(n,m) :: At
real(dp), dimension(n,n) :: AtA, invAtA
real(dp), dimension(n) :: Atb
At = transpose(A) ! transpose of A : A'
AtA = matmul(At,A) ! left-hand side multiplication of A by A': A'A
Atb = matmul(At,b) ! A'b
invAtA = inv(AtA) ! an inverse of A'A
x = matmul(invAtA,Atb) ! x = (A'A)^(-1) * A'b
end function
For this to work you need these routines to find an inverse of a 3x3 matrix:
function det_3x3(a)
! Result
real(dp) :: det_3x3
! Input
real(dp), dimension(3,3), intent(in) :: a
det_3x3 = a(1,1)*a(2,2)*a(3,3)+a(1,2)*a(2,3)*a(3,1)+a(1,3)*a(2,1)*a(3,2) &
-a(1,3)*a(2,2)*a(3,1)-a(1,2)*a(2,1)*a(3,3)-a(1,1)*a(2,3)*a(3,2)
end function
!=======================================================================
function adj(a)
!
! Adjugate of a 3x3 matrix (the transpose of the cofactor matrix).
!
! Result
real(dp), dimension(3,3) :: adj
! Input
real(dp), dimension(3,3), intent(in) :: a
adj(1,1) = a(2,2)*a(3,3) - a(2,3)*a(3,2)
adj(1,2) = a(1,3)*a(3,2) - a(1,2)*a(3,3)
adj(1,3) = a(1,2)*a(2,3) - a(1,3)*a(2,2)
adj(2,1) = a(2,3)*a(3,1) - a(2,1)*a(3,3)
adj(2,2) = a(1,1)*a(3,3) - a(1,3)*a(3,1)
adj(2,3) = a(1,3)*a(2,1) - a(1,1)*a(2,3)
adj(3,1) = a(2,1)*a(3,2) - a(2,2)*a(3,1)
adj(3,2) = a(1,2)*a(3,1) - a(1,1)*a(3,2)
adj(3,3) = a(1,1)*a(2,2) - a(1,2)*a(2,1)
end function
!=======================================================================
function inv(a)
!
! An inverse of a 3x3 matrix.
!
! Result
real(dp), dimension(3,3) :: inv
! Input
real(dp), dimension(3,3), intent(in) :: a
! Locals
real(dp) :: detr
detr = 1./det_3x3(a)
inv = adj(a)*detr
end function
I use double precision for reals:
integer, parameter :: dp = kind(1.0d0)