雅克比矩阵

今天药忘吃喽~ 2024-04-17 22:43 51阅读 0赞

### 文章目录 ###

*  雅克比矩阵推导
 *  雅可比矩阵的代码实现

计算雅克比矩阵

# 雅克比矩阵推导 #

![在这里插入图片描述][20190902150348282.png]  
![在这里插入图片描述][20190902150527444.png]  
函数h(x)相对于x的导数，被称作雅可比式。  
![在这里插入图片描述][20190902145957220.png]  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70]

推到过程如下：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 1]  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 2]  
为了计算这个函数的导数，我们使用了商的求导规则。

给定一个函数z，它是另外两个函数f和g的商：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 3]  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 4]

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 5]  
注意：必须保证雅可比矩阵的除数（分母）不为零。

# 雅可比矩阵的代码实现 #

#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

[20190902150348282.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/04/17/1ed5b104b5474b1d80c38ba101986700.png
[20190902150527444.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/04/17/95ea84799a51403abcfafe4ed029b744.png
[20190902145957220.png]: https://image.dandelioncloud.cn/pgy_files/images/2024/04/17/ff819c31f51f4001a46b2ef9ee7beadb.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70]: https://image.dandelioncloud.cn/pgy_files/images/2024/04/17/99d9367dbee747dc85a9e2b3d5862d2d.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 1]: https://image.dandelioncloud.cn/pgy_files/images/2024/04/17/9d1f43fabe4648e6a3dec788fb3b6b61.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 2]: https://image.dandelioncloud.cn/pgy_files/images/2024/04/17/61cdd3ed870c4b438926a1a11d13505d.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 3]: https://image.dandelioncloud.cn/pgy_files/images/2024/04/17/6f7d07aef8ae4d358fd484e20dc7a231.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 4]: https://image.dandelioncloud.cn/pgy_files/images/2024/04/17/d24fc0042abb4dfeaa3ce6ee804fc7e0.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3hpYW9fbHhs_size_16_color_FFFFFF_t_70 5]: https://image.dandelioncloud.cn/pgy_files/images/2024/04/17/d1349afc8755405d827de77248b76711.png