雅克比矩阵

计算雅克比矩阵

雅克比矩阵推导

函数h(x)相对于x的导数，被称作雅可比式。

推到过程如下：


为了计算这个函数的导数，我们使用了商的求导规则。

给定一个函数z，它是另外两个函数f和g的商：

注意：必须保证雅可比矩阵的除数（分母）不为零。

雅可比矩阵的代码实现

#include <iostream>
#include <vector>
#include "Dense"

using Eigen::MatrixXd;
using Eigen::VectorXd;
using std::cout;
using std::endl;

MatrixXd CalculateJacobian(const VectorXd& x_state);

int main() {
  /**
   * Compute the Jacobian Matrix
   */

  // predicted state example
  // px = 1, py = 2, vx = 0.2, vy = 0.4
  VectorXd x_predicted(4);
  x_predicted << 1, 2, 0.2, 0.4;

  MatrixXd Hj = CalculateJacobian(x_predicted);

  cout << "Hj:" << endl << Hj << endl;

  return 0;
}

MatrixXd CalculateJacobian(const VectorXd& x_state) {

  MatrixXd Hj(3,4);
  // recover state parameters
  float px = x_state(0);
  float py = x_state(1);
  float vx = x_state(2);
  float vy = x_state(3);

  // TODO: YOUR CODE HERE 


 // check division by zero
 // if (fabs(c1) < 0.0001) {
 //   cout << "CalculateJacobian () - Error - Division by Zero" << endl;
 //   return Hj;
 // }
 
  // check division by zero
  if (px != 0 and py != 0)
  {
      float dt_2 = px*px + py*py;
      float dt_sqrt = sqrt(dt_2);
      float dt_sqrt_3 = sqrt(dt_2*dt_2*dt_2);
       // compute the Jacobian matrix
       Hj << px/dt_sqrt, py/dt_sqrt, 0, 0,
            -py/dt_2, px/dt_2, 0, 0,
            py*(vx*py-vy*px)/dt_sqrt_3, px*(vy*px-vx*py)/dt_sqrt_3, px/dt_sqrt, py/dt_sqrt;
      
  }
  // compute the Jacobian matrix
  
  

  return Hj;
}

运行结果如下：

Hj:
0.447214 0.894427        0        0
    -0.4      0.2        0        0
       0        0 0.447214 0.894427