CODEWITHSUNDEEP
kalman-filter
babyCoder
babyCoder

Reputation: 1

Implementation of extended kalman filter with kinematic bicycle model

I am implementing extended Kalman filter to fuse GPS and IMU sensor data. For this the state dynamics I have chosen kinematic bicycle model. For that the equation derived are as follows

state vector consists of x position, y position, heading angle control vector consist of acceleration in x, acceleration in y, steering angle

def f(x,u): 
    # x: state vector [pos_x, posy, headingAngle] 
    # u: control vector [acc_x, accy, steerAngle] 
    dt = 1 # time step 
    pos_x, posy, headingAngle = x 
    acc_x, acc_y, steerAngle = u 
    vel_curr_x = acc_x*dt 
    vel_curr_y = acc_y*dt 
    v_Curr = np.sqrt(vel_curr_x**2 + vel_curr_y**2) 
    sideSlipAngle = math.atan2((
        (math.tan(steerAngle),/wheelBaseLength)*rearwheelBase
    ) 
    vel_X = v_Curr * math.cos(headingAngle + sideSlipAngle) 
    vel_Y = v_Curr * math.sin(headingAngle + sideSlipAngle) 
    rate_HeadingAngle = v_Curr*(((math.tan(steerAngle))*math.cos(sideSlipAngle))/wheelBaseLength)
    #State Equations 
    new_pos_x = pos_x + vel_X*dt 
    new_posy = posy + vel_y*dt 
    new_heading = headingAngle + rate_HeadingAngle * dt

To proceed, I have calculated matrix F, which is state transition matrix resulting in 3x3 identity matrix. (Jacobian of f(x,u) w.r.t state vector x).

I am stuck at calculating the control matrix B, (Jacobian of f(x,u) w.r.t input vector u.)

My problem is that due to dependency of variables on one another, partial derivative of this equation, the results in a complicated equation which is difficult to express in standard EKF equation of

x = Fx + Bu.

My question is, in EKF is it sufficient to proceed with taking partial derivative of state equation w.r.t. input state vector? If not then how should I proceed with taking Jacobian in a right way.

Upvotes: 0

Views: 664

Answers (1)

kartiks77
kartiks77

Reputation: 72

In my experience, EKF works well for systems with moderate non linearity. Taking partial derivates is the common way to linearize the system equations. Since you update the system's linearized dynamic matrix and the control matrix at each step of filtering, you get a fairly good state prediction.

For highly non-linear systems, I would advise you to try out UKFs. They provide more accurate estimations compared to EKF for systems of increasing non-linearity. An excerpt from the book: Kalman Filtering by Grewal and Andrews

Upvotes: 0

Related Questions