激光雷达定位算法在FPGA中的实现——section2 全局坐标和角度计算

1、全局坐标矩阵X

        在section1中已经就激光雷达的全局坐标公式进行了推导，得到

          ​

        其中，           

        

       

        这里需要计算出矩阵X里面的x，y，cosα，sinα。

        关于这个计算公式的求解，方阵的逆矩阵计算成为关键步骤。

2、方阵的逆变换

2.1 4*4方阵C简化

        这里把4*4的方阵C简单化：

          ​​​​​​​，

        其中：

        

        

        

        

        

        

        

        

        

        

        

        

        

        

        

        

2.2 4*4方阵C余子式矩阵C_1

        定义方阵C的余子式矩阵C_1：

          ​​​​​​​

        其中：

        ​​​​​​​

        

2.3 矩阵C_1代数余子式矩阵C_2

2.4 矩阵C_2伴随矩阵C_3

2.4 矩阵C_3乘以行列式倒数后的矩阵C_4

2.5 方阵C的逆矩阵C_4

2.6 全局坐标矩阵结果