The question of head file "csv_reader.hpp" and member function "void EKF::updateJA(const double dt)" in the class of ekf

[karanchawla]

The question of head file "csv_reader.hpp" and member function "void EKF::updateJA(const double dt)" in the class of ekf

[liyuebit]

Hello,
Have you solved your problem？

[863217373]

No

[karanchawla]

Will push a fix by tonight. It's basically the jacobian of the matrix.

[karanchawla]

Apologies for the delay,

here's how you can compute the jacobian of the dynamic matrix A

# Calculate the Jacobian of the Dynamic Matrix A
    # see "Calculate the Jacobian of the Dynamic Matrix with respect to the state vector"
    a13 = float((x[3]/x[4]) * (np.cos(x[4]*dt+x[2]) - np.cos(x[2])))
    a14 = float((1.0/x[4]) * (np.sin(x[4]*dt+x[2]) - np.sin(x[2])))
    a15 = float((dt*x[3]/x[4])*np.cos(x[4]*dt+x[2]) - (x[3]/x[4]**2)*(np.sin(x[4]*dt+x[2]) - np.sin(x[2])))
    a23 = float((x[3]/x[4]) * (np.sin(x[4]*dt+x[2]) - np.sin(x[2])))
    a24 = float((1.0/x[4]) * (-np.cos(x[4]*dt+x[2]) + np.cos(x[2])))
    a25 = float((dt*x[3]/x[4])*np.sin(x[4]*dt+x[2]) - (x[3]/x[4]**2)*(-np.cos(x[4]*dt+x[2]) + np.cos(x[2])))
    JA = np.matrix([[1.0, 0.0, a13, a14, a15, 0.0],
                    [0.0, 1.0, a23, a24, a25, 0.0],
                    [0.0, 0.0, 1.0, 0.0, dt, 0.0],
                    [0.0, 0.0, 0.0, 1.0, 0.0, dt],
                    [0.0, 0.0, 0.0, 0.0, 1.0, 0.0],
                    [0.0, 0.0, 0.0, 0.0, 0.0, 1.0]])

This is a python implementation. So, you'll have to convert it to C++

This is based off of work from @balzer82.

[HandsomeDragon3]

There might be a mistake that you wrote" const double r13 = turn_radius * (-cos(dt * psi_dot) + psi);" in utils.cpp

[balzer82]

@karanchawla Would be nice, if you mention the Author of this code snipped, because I released it under CC-BY 2.0 License.

[karanchawla]

Definitely. Apologies for not doing that. Will add it to the Readme as well.