fix: Included headers in cli/ instead the one in test/ for the test program

Signed-off-by: Sebastian Niemann <niemann@sra.uni-hannover.de>
test: Reduced the amount of sample points to test, as more points does not increased the test coverage

Signed-off-by: Sebastian Niemann <niemann@sra.uni-hannover.de>
test: Renamed some variables to be more consistent between the Mtlab and C++ code

Signed-off-by: Sebastian Niemann <niemann@sra.uni-hannover.de>
test: Added tests verifying the correctness of the implementation

Also, deleted the dummy test code.

Signed-off-by: Sebastian Niemann <niemann@sra.uni-hannover.de>
devel: Made all setter virtual

Signed-off-by: Sebastian Niemann <niemann@sra.uni-hannover.de>
devel: Added the keyword "override" to each function where this happens

Signed-off-by: Sebastian Niemann <niemann@sra.uni-hannover.de>
devel: Reduced the scaling matrix to a column vector and used elemt-wise multiplication to speed up the calculation

Signed-off-by: Sebastian Niemann <niemann@sra.uni-hannover.de>
devel: Some minor corrections

Signed-off-by: Sebastian Niemann <niemann@sra.uni-hannover.de>
fix: Fixed remaining errors in the calculation of the objective function

Everything is now passing the tests and works as intended.

- Fixed #13
- Fixed #14
- Fixed #15
- Fixed #16
- Fixed #17
- Fixed #4

Signed-off-by: Sebastian Niemann <niemann@sra.uni-hannover.de>