# control over changing data

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,

1. 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.

1. 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?

2. 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?

3. 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}$$

1. What changes do I have to (or need to make), to [...] track a moving point?

In the state-space representation I think this is typically done with reference tracking in the form of "Integral Control" or "Integral Action." You essentially wrap an integral controller around your state feedback controller and pass the integral error. If I recall correctly you extend your state vector to include integral error, and then the state feedback controller drives all states to zero, including the integral error, et voila.

1. 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?

You don't get to selectively butcher your state vectors. They're either the states, or they're not. Use your B matrix to map your inputs to state derivatives, use your C (and D) matrices) to map your states back to outputs, but you can't "get rid of" one of the states.

1. 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?

Well you can't really do that, right? If it's a constant input then are you actually controlling anything? The mechanism you might be tempted to use would be the almost-always-zero D matrix, which maps INPUTS directly to y/output, but that's not the "kind" of output you would use for control. The y outputs are those that are the measurable outputs, so for example if you had vehicle speed as a state but you only had wheel encoders, the wheel encoder reading should depend on the vehicle speed - the C matrix would contain the math that converts from vehicle speed to wheel encoder speed so you can do some comparisons between y and your sensor feedback. Y is not the control output.

What you're looking for instead is a kind of feed-forward control, but again I don't think what you're asking about is what you actually want.

1. Which of the two approaches is more accurate? Assuming all robots are on the ground.

I don't understand this question.

In general, I think you've got kind of the wrong approach here in trying to do speed and heading together in one controller. If it were me, I'd split them out into a heading controller and a speed controller, then I'd set the poles on the heading controller to be (arbitrarily) 5 to 10x faster than the poles on the speed controller, so you get to heading faster than you get to position, which is kind of required if you're ever going to get to the correct position. Then you can have your inputs as r/$$\theta$$. Your control outputs for the heading controller would be some $$\ddot{\theta}$$ that in turn maps symmetrically to the left and right wheels, such that v_L = (-v_R).

The output of the speed controller would pipe to each wheel, and then you sum the outputs of both controllers to get the complete system.

• I would clarify speed and heading - there is translation velocity and angular velocity. These 2 velocities are, in the general case, independent of each other. If that statement is not true, that is because there are robot kinematics not explained in the question. In other words, I agree with what @Chuck said, 'speed' and 'heading' are not the same controller. Mar 9 at 0:04
• @guthik - Kind of a lot to unpack here. First, the C and D matrices are part of the state space model, so I don't know what you're asking here. If your measurable outputs are the same as your states then C is identity and D is zero. You're talking about a "distance between robots" now, which is not part of your original post. If you want them to do the same thing then what you might consider instead is a kind of hybrid control, where you use feed-forward to give robot #2 the same input as #1, so they do the same thing, then you can operate on some arbitrary position error that results. Mar 14 at 13:33
• What I'm describing here is probably more suited for PID control instead of state feedback controls. Mar 14 at 13:37
• @guthik The big advantage you get with state feedback control is the ability to model some kind of a (linear) process, so you can use your knowledge of how the process responds to tailor the inputs to get what you want. The advantage to using feed-forward here is, if your two vehicles are identical, you basically get to skip modeling the lead vehicle because you can just pass the same input to the following vehicle and get nearly identical output. If you want to do it with state feedback then you've got to try to model all of that but you get it for free with a second set of hardware. Mar 14 at 19:37
• @guthik now you're getting into a topic called "swarm robotics," but I think generally the same concept would apply - send the same command to all robots as a "coarse" adjustment, and then all robots can have their own controllers for "fine" adjustment. You could have a PID controller that monitors distance between self and the N+1 vehicle and another PID controller that monitors distance between self and the N-1 vehicle, or you could have one controller that operates on the half-distance between N+1 and N-1, or you could have one controller that operates on the (N-1)th gap, etc. Mar 24 at 14:16