Used the non linear dynamics to implement the non linear MPC. This non linear MPC uses non linear optimization tool in matlab that is fmincon. The initial input to this MPC is the initial position of the drone and the optimization task is to generate a control inputs which satisfy the constraints as well as the drone reaching the goal position.
This non linear MPC runs until all the control inputs needed to reach the goal position are generated. It has a prediction horizon of 20 which means that after every optimization step, there will be 20 control inputs of which 1st control input is taken and applied to the drone by forward simulating the drone dynamics using fourth-order runge-kutta method. On doing this the new state is received which is then fed back to the MPC as its initial state and this is performed until drone reaches the goal position.
In the cost function, the error in the state is addressed while simultaneously imposing penalties on high acceleration and input actions. This approach aims to mitigate excessively high acceleration maneuvers, which are impractical for the quad-copter’s capabilities. Conversely, no penalties imposed on the quadcopter’s velocity, as the objective is to enable it to reach the goal state as quickly as possible. The parameters R, Q, and N were manually adjusted through a process of trial and error to achieve smooth performance.
