Fix the test data matrix script

[jmackay2]

Solved Problem

The matrix test_data.py script did not work.

Solution

• Add python3 shebang
• Remove std::sqrt and replace with a python sqrt (**0.5)
• Add a float epsilon

Alternatives

We could use a python package that does sqrt if we want to be more verbose, but the square function just before is just using python square, so it is probably not necessary.

Test coverage

• Ran the test_data.py script successfully with the outputs printing correctly

[github-actions]

This PR was identified as stale and it will be closed in 30 days unless any activity is detected.

[jmackay2]

For some reason the previous build had a failure that didn't seem related. This should still be a valid fix and not stale.

[jmackay2]

The SITL errors are unrelated to the change.