function [A, B] = vppblock_StateJacFcn(x, u, wb)
%#codegen
% Jacobian of model equations for Vehicle Path Planner (VPP) block using
% multistage nonlinear mpc.
%
% state variables: positions x, y and yaw angle theta.
% control variables: v and steering angle delta.
%
% To customize the Vehicle Path Planner (VPP) block, specify a state
% Jacobian function for the nonlinear mpc object (nlobj). For example:
%
%       [A, B] = myStateJacFcn(x, u, p) 
%       nlobj.Model.StateJacFcn = "myStateJacFcn";
%
% The state Jacobian function (when specified) must be updated when the
% state function is updated.
%
% see also: vppblock_CreateNonlinearMPC, vppblock_StateFcn

% Copyright 2022 The MathWorks, Inc.

%%
% Variables
theta = x(3);
v = u(1);
delta = u(2);

% Linearize the state equations at the current condition
A = zeros(3,3);
B = zeros(3,2);

A(1,3) = -v*sin(theta);
B(1,1) = cos(theta);

A(2,3) = v*cos(theta);
B(2,1) = sin(theta);

B(3,1) = 1/wb*tan(delta);
B(3,2) = 1/wb*(v*(1+tan(delta)^2));
