# How do I derive complicated robotic motion models easily?

I have a filter that tracks a robot. I want it to use a 2D coordinated turn polar velocity motion model (from page 15 here):

But I want to expand on this motion model: I want an additional velocity state that is orthogonal to the heading, and I also want acceleration states for the two velocities.

How do I find the resulting discrete-time linearized model easily? I guess I could someone smarter than me could find the find the continuous time differential equation by hand (by thinking really hard?), and then discretize/linearlize (or linearlize/discretize) it. But that seems like an awful amount of work. I think I can manage the "discretize/linearlize"-step, but I don't know how to start on the "find the continuous time model" step.

It would be nice if there were some program that takes my desired states as input and outputs a model. But my googling turns up nothing. Is it really hard to find motion models? Do I fail to see the gravity of the work if I feel like this could be done in an afternoon? What should my workflow look like? Where do I start?

• Also, I'm having a hard time placing this question. Is it math? Physics? Maybe it should be at CrossValidated? Or Robotics? If anyone has input here, I'd be happy! – blåblomma May 25 '19 at 19:58
• Well Blue Flower, I'm not that familiar with Kalmans so I'm not sure I can help, but your entreaty to "make the complicated be done easily" challenges me to read the paper. First glance says be careful with tan^{-1}(y/x), you want to use atan2(y,x) or unexpected results may ensue. Orthoganal to a direction in 3D means a plane, did you perhaps mean to constrain it to the osculating plane? – Cedron Dawg May 25 '19 at 23:40
• If all you are looking for is a continuous mathematical model for a set of discrete inputs to match a set of outputs then "polyfit" is probably your best answer. It is a generalized function which produces a set of polynomial coefficients for a best fit solution. The nice thing about it being polynomial is that the coefficients for the derivative functions can be calculated directly from the results. – Cedron Dawg May 26 '19 at 12:12
• Thanks for your replay @CedronDawg! The entire problem is in 2D, so by "orthogonal", I mean "not in the direction of heading/$\phi$". So I mean to constrain it to the plane. I think that the "discretize/linearlize" is the easy part. I'm not 100% sure about how to do it, but I guess something like "polyfit" would work well. The hard part seems to be to make the continuous mathematical model. I don't knmow how to do that or where to start. :s – blåblomma May 27 '19 at 8:05
• First, orthogonal in 2D is easy. Simply reverse the coordinates and negate one of them. Second, if you can list your inputs and outputs as columns in a comma separated file, I have written a Python program that applies the polyfit and gives you the right coefficients. It's a little longish because it also generates C, Python, and VB code to implement the model. Send me an email to cedron at exede dot net and I can send you a copy. – Cedron Dawg May 27 '19 at 11:49