Are you looking for help or someone to do it for you?
I don't mean to be rude, but what you are asking for is usually graduate-level work. If you have specific questions about Kalman filters I would love to help. If you have specific questions about modeling the system dynamics I am sure that you would generate lots of discussion. Asking for code will likely get you nowhere.
Yea you can get a PhD in Kalman fiters. There are Text Books out there with Code on CDs in the back.
I took a class at Fullertion St from one of the best. Dr. Grewal. I suggest you buy it