No respond from Arduino , sketch within memeory limits.

Hello
I've got DcDuino board, fully compatibile with Arduino UNO .
I'm trying to load sketch that uses kalman filter to estimate attitude. It uses 6x6 matrices.
Code between marks " //// [[[[]]]] //// " has been checked in visual studio and is vorking perfectly good.
But when I want to load it into arduino, serial port print nothing. Only L LED blinks one, showing that setup{} loop has been initialized.

My previous project was Kalman filter for 2x2 matrices, which was working superb. I can load it, and serial prints everything I want.

I'm using MatrixMath library from arduino playground, it has been checked in visual studio of course.

Any ideas what is not working? Too many operations?

CODE : C++ code - 294 lines - codepad

I think it is very unlikely that a kalman implementation using 6x6 matrices will fit into the 2k ram on a Uno. Mine uses 4k on a mega.

And it's slow.

Actually, in Arduino IDE it says:
Sketch size 15,652 bytes (of a 32,256 byte maximum)

Yeah, it's slow.. but Its only for testing a conception

Ok, so..
Do you think going to MEGA is a way to go around this problem?
I want to build up my setup on Raspberry Pi, but it is running on linux - needs more work that just throwing code on the board.

EDIT : // You were right! Arduino mega is eating my code, working fine .. but slow :wink: Loop time is about 0.045 sec. , so it has no application but just academical research.