About the system model and non-quadratic objective function

[BlueeHole]

First of all thank you for sharing this code! But I have a question for this.
The vehicle model you use in car.py is non-linear.

def update_state(self, dt):
		A = np.array([
				[1, 0, 0],
				[0, 1, 0],
				[0, 0, 1]
					])
		B = np.array([
				[np.sin(self.x[2, 0] + np.pi/2)*dt,  0],
				[np.cos(self.x[2, 0] + np.pi/2)*dt,  0],
				[0			 , dt]
					])

		vel = np.array([
					[self.x_dot[0, 0]],
					[self.x_dot[2, 0]]
				])
		self.x = A@self.x + B@vel

In the MPC objective function, you directly use this model to derive all future states within the control hoziron, and use QP to solve it.

x, _ = controller_car.get_state()
z_k[:,i] = [x[0, 0], x[1, 0]]
cost += np.sum(self.Q@((desired_state-z_k[:,i])**2))

But in this case, the objective function is not quadratic, right? (it contains sin(x) ).
Is there any theoretical basis for doing this? Or did I misunderstand something? Hope for your responding, thank you!

[zainkhan-afk]

Hi, The car model is constrained to only move in one direction (forward). This means that the motion in the other planar direction is constrained, in this case I have assumed that "forward" is towards the y-axis of the robot. Therefore, the velocity of the robot will be applied in the y direction when its heading is 0, however, when the robot heading is non-zero, the robot will move at an angle and the robot displacement will be in both x and y axis, in other words, when looking at the robot from the world frame, it will appear to have velocity in both x and y axis. To calculate this velocity I take the sine and cosine of the robot heading, this gives me a multiplier (scaling value) that I multiply with the current robot velocity to make it move according to the heading. You can think of the result after ***@***.*** to be equal to: [ [delta_y], [delta_x], [delta_theta] ] The cost function is a quadratic equation, I am summing the squares of the variables and their differences.

…

On Tue, Mar 26, 2024 at 5:24 PM 莫非 ***@***.***> wrote: First of all thank you for sharing this code! But I have a question for this. The vehicle model you use in car.py is *non-linear*. def update_state(self, dt): A = np.array([ [1, 0, 0], [0, 1, 0], [0, 0, 1] ]) B = np.array([ [np.sin(self.x[2, 0] + np.pi/2)*dt, 0], [np.cos(self.x[2, 0] + np.pi/2)*dt, 0], [0 , dt] ]) vel = np.array([ [self.x_dot[0, 0]], [self.x_dot[2, 0]] ]) self.x = ***@***.*** + ***@***.*** In the MPC objective function, you directly use this model to derive all future states within the control hoziron, and use QP to solve it. x, _ = controller_car.get_state()z_k[:,i] = [x[0, 0], x[1, 0]]cost += np.sum(self.R@(u_k[:,i]**2)) But in this case, *the objective function is not quadratic*, right? (it contains sin(x) ). Is there any theoretical basis for doing this? Or did I misunderstand something? Hope for your responding, thank you! — Reply to this email directly, view it on GitHub <#3>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AIUWJZIJM3LHTH6YC4PKYA3Y2FSHRAVCNFSM6AAAAABFI2GUMGVHI2DSMVQWIX3LMV43ASLTON2WKOZSGIYDQMJQGQ2TONI> . You are receiving this because you are subscribed to this thread.Message ID: ***@***.***>

[BlueeHole]

Thank you for your detailed response! But I still have one question. quadratic should be polynomial, right? But here, at every step, you update x using x\dot=Ax+Bu, here A is non-linear of x. ( because sin(\theta) ). So from the second step of update, u has been coupled with x, and the whole cost function has been non-linear of u. Therefore, the sum of these items would be non-linear of u.
That is, the objective function is non-linear. Here is a misunderstanding. I firstly thought SLSQP is for quadratic(polynomial) objective function, but it actually can solve non-linear functions.( from the documentation of scipy)

So if I had discovered that SLSQP could be used to solve nonlinear problems from the beginning, I would not have raised this issue. But I happened to have not discovered it before, that's 'lucky', to some degree.
To sum up, my question is , whether your cost function is a polynomial of u? I think it's not, so it's not quadratic.
I searched other materials and found many people do linearization like this: (sorry it's Chinese, but please check the formula)

that is to use Taylor expension on the reference point. Here the θr is a constant (the reference theta )As I think, through this the objective function would be linear and really 'quadratic'. But by this method, we need to provide θ_r and v_r on every point, since A contains θ_r and v_r.