function [Ag,Bg,Cg] = adasblocks_utilGetLatVehModelFromParam(m,Iz,lf,lr,Cf,Cr,Vx)
%#codegen
%
% Get state-space continuous vehicle model from vehicle parameters with
% input: steering angle 
% outputs: lateal velocity and yaw angle rate
%
% Parameters
%       m: vehicle mass 
%      Iz: yaw moment of inertia
%      lf: longitudinal distance to the front axis
%      lr: longitudinal distance to the rear axix
%      Cf: cornering stiffness of front tires
%      Cr: cornering stiffness of rear tires
%      Vx: longitudinal velocity
% Vehicle state-space model with state varaibles x = [vy,r]. 
%      vy: lateral velocity
%       r: yaw angle rate

%   Author: Meng Xia, Rong Chen
%   Copyright 2017-2018 The MathWorks, Inc.

% Specify dynamics: dx = [a1,a2;a3,a4]x + [b1;b2]u; y = x.
a1 = -(2*Cf+2*Cr)/m/Vx;
a2 = -(2*Cf*lf-2*Cr*lr)/m/Vx - Vx;
a3 = -(2*Cf*lf-2*Cr*lr)/Iz/Vx;
a4 = -(2*Cf*lf^2+2*Cr*lr^2)/Iz/Vx;
b1 = 2*Cf/m;
b2 = 2*Cf*lf/Iz;
% Specify state-space matrices.
Ag = [a1,a2;a3,a4];
Bg = [b1;b2];
Cg = eye(2);