function cost = vppblock_CostFcn(stage, x, u, du, paras)
%#codegen
% Cost function for Vehicle Path Planner (VPP) block using multistage
% nonlinear mpc. 
%
% To customize the Vehicle Path Planner (VPP) block, specify a cost
% function for the nonlinear mpc object (nlobj). For example, use the
% specified cost fucntion at stage k:
%
%       cost = myCostFcn(stage, x, u, du, paras); 
%       nlobj.Stages(k).CostFcn = "myCostFcn";
%
% To enforce terminal constraint, a terminal cost can be specified in the
% cost function compared to using TerminalState for the nlobj.
%
% see also: vppblock_CreateNonlinearMPC

% Copyright 2021-2022 The MathWorks, Inc.
    
Wmv = eye(2);
cost =  u'*Wmv*u;


