雅克比矩陣

計算雅克比矩陣

雅克比矩陣推導

函數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