I am trying out different control algorithms, in my practice on the state space models and control. Now I have the following system with the following states and state transitions with time.
Position variation with time:
$$
\begin{equation}
\begin{bmatrix}
x_t\\ y_t \\ \theta_t
\end{bmatrix} =
\begin{bmatrix}
x_{t-1} + v_{t-1}cos(\theta_{t-1}) \delta t \\
y_{t-1} + v_{t-1}sin(\theta_{t-1}) \delta t \\
\theta_{t-1} + \omega_{t-1} \delta t
\end{bmatrix}
\end{equation}
$$
x and y
are coordinates in the world, $\theta$ is the bearing of my robot. v
is its velocity and dt
is the change in time.
In order to have to robot go somewhere, I linearized the above system into the form, $X_t = A_{t-1}X_{t-1} + B_{t-1}u_{t-1}$, using the linear and angular velocities as inputs to control the position of the robot to have it go to desired location. Using the LQR algorithm. Current state is position and desired state is in form of position as well at the moment. My questions are,
- What changes do I have to (or need to make), to have the robot not move to a stationary position but track a moving point? Assuming i retrieve the state information from sensors periodically. Am a bit confused here because, when calculating the discrete ricatti equation, I have something like this:
For i = N, …, 1
P[i-1] = Q + ATP[i]A – (ATP[i]B)(R + BTP[i]B)-1(BTP[i]A)
I am doing it in python by the way, for what its worth.
How do my matrices change, if I choose to move in one direction, say get rid of the y coordinate. So my robot moves in a straight line?
What effects does changing the current and desired state from position (coordinates), to something constant, say distance? Is this even advisable? if not, why not? Still i will forward distance to controller as state over time, and controller will give me velocity. How do my matrices change according to the distance set up?
Which of the two approaches is more accurate? Assuming all robots are on the ground.
Below are my A and B
matrices
$$
\begin{equation}
A =
\begin{bmatrix}
1 & 0 & 0 \\
0 & 1 & 0 \\
0 & 0 & 1
\end{bmatrix},
B =
\begin{bmatrix}
cos(\theta_{t-1}) \delta t & 0 \\
sin(\theta_{t-1}) \delta t & 0 \\
0 & \delta t
\end{bmatrix}
\end{equation}
$$
Thanks a lot in advance.