How to Convert State Equations to a State Transition Matrix for a Kalman Filter?

[docsxp]

I have my state vector containing
$$[X, Y, v_x, v_y, \theta, r, a_x, a_y, b_{\theta}]^T$$

and I have them related by
$$dX = v_x cos \theta - v_y sin \theta\\
dY = v_x sin \theta + v_y cos \theta\\
dv_x = a_x\\
dv_y = a_y\\
d\theta = r\\
dr = 0\\
da_x = 0\\
da_y = 0\\
db_\theta = 0\\
$$

Now I'm actually lost in how to go about in converting them to my state transition matrix representation. Can anyone chime in and help me along please? Thank you.

 

[rrdrr8556]

in the Kalman filter, your state progress to the next iteration. how do you define that in terms of your differential quantities ? For instance, your state progressing according to:

\dot{X} = v_x cos(\theta) - v_y sin(theta) ?

I don't quite understand how you have come to the differential quantities.

 

[timthereaper]

I don't see how you can to do it with the cosine and sine functions there. You'll have to linearize about some theta. Plus, I'm backing @rrdrr8556 in that I also don't understand how you're coming up with those as the differential equations, assuming dX and dY is just bad shorthand for the differential equations of xdot and ydot.

Maybe if you post your entire problem we can help you figure out a proper model for it.

 

[docsxp]

I don't see how you can to do it with the cosine and sine functions there. You'll have to linearize about some theta. Plus, I'm backing @rrdrr8556 in that I also don't understand how you're coming up with those as the differential equations, assuming dX and dY is just bad shorthand for the differential equations of xdot and ydot.

Maybe if you post your entire problem we can help you figure out a proper model for it.

I am sorry, I should have asked the question in a proper manner. I will outline my system and derivation here.

The system is a robotic platform with a high grade 9-axis (non FOG) IMU and a velocity sensor. The system has 6-dof, but I want to assume 4-dof for simplicity as well as mechanical correction on certain axes.

Hence, my state vector, $$[X, Y, v_x, v_y, a_x, a_y, \omega, \psi, b_\psi]$$ where omega is the turn rate in z-axis (rad/s) and psi is true heading wrt true north. b is the bias (true-gyro). My equations of motion are $$X = X_{0X} + v_x t + \frac{1}{2}a_x t^2$$ $$v_x = v_{0x} + a_x t$$ $$\psi = \psi_0 + \omega_z t$$ and so on for other axes.

Hence my state transition matrix that i came up with for this is $$\left[ \begin{array}{cccc} 1 & 0 & cos\psi & -sin\psi & 0.5(dt)^2 & 0 & 0 & 0 & 0\\
0 & 1 & sin\psi & cos\psi & 0 & 0.5(dt)^2 & 0 & 0 & 0\\
0 & 0 & 1 & 0 & 1 & 0 & 0 & 0 & 0\\
0 & 0 & 0 & 1 & 0 & 1 & 0 & 0 & 0\\
0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\
0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0\\
0 & 0 & 0 & 0 & 0 & 0 & 1 & 1 & 0\\
0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0\\
0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1
\end{array} \right]$$

I don't know how to deal with the acceleration term. Any ideas?

 

[docsxp]

I think this is not how it should be done, but I should create a Jacobian matrix for F.

 

[docsxp]

Can anyone point me in the right direction?