Added initialization for estimatedState matrix.
Fixed typos while adapting code to Java and added some tests.
After correcting code in Matrix class the tests are now running.