首先介绍点基础知识，另一方面也算是巩固下： 
A−1表示A的逆矩阵; 
AT表示A的转置; 
AH表示Hermitian矩阵(A的共轭转置矩阵A∗==A)



基础





（1）迹（Trace）

eig(A)表示A的特征值





（2）行列式（Determinant）





（3）特例2*2矩阵



以上是摘自：The Matrix Cookbook 

也可参考维基百科：Matrix calculus



L1范数的次微分

L1范数不可微。但是存在次梯度，即是次微分的 

L1范数的次梯度如下： 
∂∂x||x||1=sign(x)


其中sign(x) 表示如下：  
sign(x)=⎧⎩⎨⎪⎪+1−1[−1,1]xi>0xi<0xi=0


而L1范数：
||X||1=|x1|+|x2|+⋯+|xn|


例如：x=(3,2,−5)T

故其梯度为：sign(x)=(1,1,-1)



L2范数的微分



例如：求解下面函数的偏导数： 
f(W)=12∑i,jϵSγi,j||wTiX−wTjX||22


得：
∂f(W)∂wi=∑i,jϵsγi,j(wTiX−wTjX)∗∂(wTiX−wTjX)∂wi=∑i,jϵsγi,j(wTiX−wTjX)∗XT=∑i,jϵsγi,j(wTi−wTj)∗(XXT)


注意这里得到的是行向量的形式，因此还需要对其进行转置 

以上的推倒是基于上图公式得到。。。

条件极值的求法(函数极值的求法例题)
2020-05-07 21:51:24
共10个回答
各个分量的偏导数为0,这是一个必要条件.充分条件是这个多元函数的二阶偏导数的行列式为正定或负定的.如果这个多元函数的二阶偏导数的行列式是半正定的则需要
条件极值在求极值时有一个条件等式,求条件极值通常可以构造一个函数.如原函数是f(x,y),条件等式是z(x,y),可构造F(x,y,a)=f(x,y)+az(x,y),在分别对x,y,a求偏导令为0,求
(1)令f(x,y)=$(2x+4)dx+g(y)=x^2+4x+g(y)f'y=dg(y)/dy=-2y则g(y)=-y^2+cf(1,1)=2则c=-2f(x,y)=x^2+4x-y^2-2(2)令f'x=2x+4=0f'y=-2y=0得驻点(-2,0)f''xx=2f''xy=0f''yy=-20^2-2•(-2)>0f(x,y)无极值边界x^2+y^2=4代入f(x,y)=2x^2+4x-6(-2
解:构造拉格朗日函数L(x,y,z,m)=x-2y+2z+m(x^2+y^2+z^2-1)联立Lx(x,y,m)=1+2mx=0Ly(x,y,m)=-2+2my=0Lz(x,y,m)=2+2mz=0Lm(x,y,m)=x^2+y^2+z^2-1=0解得x=±1/3y=±2/3z=±2/3所以f(x,y,z)min=f(-1/3,2/3,-2/3)=-3
一、因为z=(1/A-1/y-1/x)^(-1),代入到U=xyz中消去z,再求二阶偏导数Uxx,Uxy,Uyy,若计算得UxxUyy-(Uxy)^2>0,而且Uxx>0,则极小值存在,这样求得符合要求的x、y、z的取值烦围仅是所求点;二、令F(x,y,z)=xyy-入(1/x加1/y加1/z-1/A),则偏导数Fx=Fy=Fz=0,又1/x加1/y加1/z-1/A=0,这样四个方程联立可以求出最值!
最后三行,可以删除了!前面已经求出两组解:(1)x=-1/3,y=2/3,z=-2/3(2)x=1/3,y=-2/3,z=2/3分别代入目标函数f,第一组得到,f=-3第二组得到,f=3所以,条件极大值为-3,条件极小值为3
这并非条件极值问题,而是需要进行坐标的旋转变换问题,将坐标变换为标准形式,这才能得到长半轴a和短半轴b,从而得到椭圆的面积.
设F(x,y,z)=x-2y+2z+λ(x²+y²+z²-9)求出偏导数,得到如下方程组:1+2λx=0-2+2λy=02+2λz=0x²+y²+z²=9(这是约束条件)由前三个方程得到,y=-2x,z=2x代入第四个方程得到9x²=9∴x=±1,x=1时,y=-2,z=2∴u=9x=-1时,y=2,z=-2∴u=-9∴条件极大值为9,条件极小值为-9
你对一阶导数的理解完全正确,二阶导数跟凹凸性有关,也是对的.一阶导为0,且两阶导大于0,比如:y=x^2,在x=0处.怎么理解呢?锅啊.从上往下看,整个都是凹的,最底下那个点就是极小值点.一阶导为0,且两阶导小于0,比如:y=-x^2,在x=0处.倒扣的锅啊,凸的,最上面的点为极大值点.好学善思的精神值得称道,祝进步!
用拉格朗日乘数法,距离d的平方为f(x,y,z),然后构造拉格朗日函数,求解.(对于你给的,只需要空间几何中的d就行,lagrange可用于曲线与平面的距离)

2个自变量+拟线性
先考虑只有两个自变量的二阶拟线性方程的定解问题
a,b,c和F都是 
 的已知函数，T是xOy平面上的一条曲线
特征理论
问题
能否唯一确定函数u的各二阶偏导 
在 
 上
的值？
解
列出关于
在 
 上的方程组
求出系数行列式的结果
判断是否为特征方程、特征曲线
由定解问题的初始条件，可得
得到关于
在 
 上的方程
得到系数行列式
对于系数行列式 
曲线 
 为
非特征曲线：
 时，
 上的
只有唯一解
曲线 
 为
特征曲线： 
 时，
 上的
无解或有无穷解
为
特征方程
特征方程的其他写法
（1）dy dx写法
因为
所以特征方程又写作
解得
(2) 用曲线隐函数表达式写法
若 
 不用参数表达式，用隐函数表达式 
因为 
 也可写成参数表达式形式，所以上述式可相等
所以，在隐函数表达式下，特征方程写作
二阶方程的分类
二次曲线分类
 判别式 
 双曲线
 抛物线
 椭圆线
二阶方程分类：这里的 
是判别式，前面的是特征方程
设 
 是一个区域， 
abcdegf都是关于xy的已知函数
方程在某点 
 处为：
双曲型偏微分方程： 
抛物型偏微分方程： 
椭圆型偏微分方程： 
对于某区域：
方程在某点领域：双曲、椭圆形偏微分方程在某点的领域类型，与在该点的类型相同。但抛物型则不知。
方程在区域 
 的类型：
混合型方程：方程在
的一个子区域为双曲型，另一个子区域为椭圆形
退化双曲型方程：方程在
的一个子区域为双曲型，在其余点（不一定构成子区域）为抛物型
退化椭圆形方程：放在在
的一个子区域为椭圆型，在其余点（不一定构成子区域）我抛物型
方程标准型
abcdeg均为常数，a不等于0
分析
换元得到新方程+求解特征方程：
令 
则
代回原泛定方程，得 
        其中
发现A、C均为特征方程形式
由特征方程 
 可得 
双曲型方程
可从特征方程的解求得两族特征线，
令
则这个变换满足特征方程，从而使得A、C均等于0
由此得到双曲型方程的第一标准型：
令 
 ,用求解第一标准型相同的方法
可得第二标准型
抛物型
因为 
 所以只能从特征方程求得一族双曲线 
所以令 
此时 
 可以令A = 0， 
 可以令B = 0
同求双曲线第一标准型的方法，得到抛物型方程的标准型
椭圆形
此时特征方程没有实解，但有虚解 
可以求得两族虚特征线
为了消除复数，做如下变换
按照求双曲型第一标准型的方法，可得椭圆型方程的标准型：
多个自变量+线性
有如下定解问题
S为曲面： 
a、b、c、f均是已知的关于 
 的函数
特征理论
问题
唯一确定u的各二阶导数 
在S上的值
解
（1）当S为超平面时，设该超平面为 
(超平面：一个n维中n-1维的子空间，可以把n维空间分成不相交的两部分)
写出定解问题
从初值条件可以得出 
  ,
从泛定方程求得 
从 
 讨论S是否为特征曲面
定解问题如下
由边界条件 1 可以求出 
  ，边界条件 2 可以求出
剩下 
 未知
求 
，由泛定方程
S为非特征曲面：当 
 时， 
 有唯一解
S为特征曲面：当 
 时， 
 无解或有解，有解需满足如下相容性条件
（2）当S为一般曲面时（这里让S为正规曲面）
将一般曲面变换为超平面：换+验证
写出新空间下的定解问题
同上述超平面解法
1
将S进行拓扑变换，将S由空间 
 转移到空间 
 ，该新空间要满足：
S在新空间里是超平面 
已知 
 可得 
已知 
 可得 
作如下变换：
证明该平面符合要求：
因为S是正规曲面，所以对任意点 
 有 
即 
由反函数定理，可得已知 
 时可求得 
23
在可逆变化下，仍变为一个二阶线性方程
在新空间中的定解问题如下：
可求出 
 与 
 ，剩下 
 未知
因为
其中， 
S为非特征曲面：当 
 唯一解
S为特征曲面：当 
 无解或满足相容性条件时无穷解
 为特征方程、
特征方程的其他形式
（?）曲面S法线方向余弦：
所以特征方程又写作 
分类与标准型
仅考虑主部具有常系数的多个自变量的二阶线性偏微分方程
a均为常数， 
方程标准型
概念解释： 
特征方程： 
特征二次型： 
二次型：n个变量的二次多项式。
二次型的标准型：二次型自变量经过 
 变化后，变成二次项里只有平方项的形式
例子
求解方程标准型：
令特征二次型 
  （因为a是常数所以矩阵A全是常数）
将特征二次型化为标准型：
     其中 
 是关于λ的对角矩阵，其中λ取值为0、1、-1
令 
 ，从而将原泛定方程转化为
得到多个自变量的二阶线性方程的标准型（主部是常系数）
根据标准型分类
椭圆、双曲型：λ全不为0
椭圆型偏微分方程：λ全是1或全是-1
双曲型偏微分方程：
λ其中一个为1，另外n-1个为-1
λ其中一个为-1，另外n-1个位1
超双曲型偏微分方程：λ取1和-1的个数都超过一个（说明自变量个数大于等于4个）
抛物型偏微分方程：λ中有一个为0，其余全为1或-1

导航


1 导航与滤波
1.1 雅可比矩阵示例

雅可比矩阵：用来预测模型？还是求取导数？

在向量分析中，雅可比矩阵(也称作Jacobi矩阵)是函数的一阶偏导数以一定方式排列成的矩阵，当其为方阵(square matrix)时，其行列式称为Jacobi行列式。要注意的是，如果雅可比矩阵为方阵，那在英文中雅可比矩阵跟Jacobi行列式两者都称作 Jacobian。在卡尔曼滤波设计的过程中往往需要对观测函数进行求取偏导，这里就可以使用雅可比矩阵表示偏导矩阵。

对于下列函数

$x=r \sin \theta \cos \varphi$

$y=r \sin \theta \sin \varphi$

$z=r \cos \theta$

求雅可比矩阵如下：

$\mathbf{J}_{\mathbf{F}}(r, \theta, \varphi)=\left[\begin{array}{ccc}{\frac{\partial x}{\partial r}} & {\frac{\partial x}{\partial \theta}} & {\frac{\partial x}{\partial \varphi}} \\ {\frac{\partial y}{\partial r}} & {\frac{\partial y}{\partial \theta}} & {\frac{\partial y}{\partial \varphi}} \\ {\frac{\partial z}{\partial r}} & {\frac{\partial z}{\partial \theta}} & {\frac{\partial z}{\partial \varphi}}\end{array}\right]=\left[\begin{array}{ccc}{\sin \theta \cos \varphi} & {r \cos \theta \cos \varphi} & {-r \sin \theta \sin \varphi} \\ {\sin \theta \sin \varphi} & {r \cos \theta \sin \varphi} & {-r \sin \theta \sin \varphi} \\ {\sin \theta \sin \varphi} & {r \cos \theta \sin \varphi} & {r \sin \theta \cos \varphi} \\ {\cos \theta} & {-r \sin \theta} & {0}\end{array}\right]$


为作动力学参考，我们假设飞行器，在月球着陆时的动力学方程如下：

$\begin{aligned} \frac{d \dot{x}}{d t} &=-\frac{\mu}{r^{3}} x+\frac{3}{2}\left(\frac{\mu C_{20}}{r^{5}}\right) x\left(1-5\left(\frac{z}{r}\right)^{2}\right)+\Omega_{e}^{2} x+2 \Omega_{e} \dot{y}+\ddot{x}_{r e s} \\ \frac{d \dot{y}}{d t} &=-\frac{\mu}{r^{3}} y+\frac{3}{2}\left(\frac{\mu C_{20}}{r^{5}}\right) y\left(1-5\left(\frac{z}{r}\right)^{2}\right)+\Omega_{e}^{2} y-2 \Omega_{e} \dot{x}+\ddot{y}_{r e s} \\ \frac{d \dot{z}}{d t} &=-\frac{\mu}{r^{3}} z+\frac{3}{2}\left(\frac{\mu C_{20}}{r^{5}}\right) z\left(3-5\left(\frac{z}{r}\right)^{2}\right)+\ddot{z}_{r e s} \end{aligned}$

关于雅可比矩阵的计算可以参考：


直接法

$\begin{aligned} \mathbf{J}(\boldsymbol{\xi}) &=\mathbf{J}_{0} \cdot \mathbf{J}_{1} \cdot \mathbf{J}_{2} \cdot \mathbf{J}_{3} \\ &=\frac{\partial \mathbf{I}_{2}\left(\mathbf{p}^{\prime}\right)}{\partial \mathbf{p}^{\prime}} \cdot\left[\begin{array}{ll}{f_{x}} & {0} \\ {0} & {f_{y}}\end{array}\right] \cdot\left[\begin{array}{ccc}{1} & {0} & {-\frac{X^{\prime}}{2 !}} \\ {0} & {1} & {-\frac{Y^{\prime}}{Z^{\prime}}}\end{array}\right] \cdot\left[\begin{array}{cccccc}{1} & {0} & {0} & {0} & {Z^{\prime}} & {-Y^{\prime}} \\ {0} & {1} & {0} & {-Z^{\prime}} & {0} & {X^{\prime}} \\ {0} & {0} & {1} & {Y^{\prime}} & {-X^{\prime}} & {0}\end{array}\right] \cdot \frac{1}{Z^{\prime}} \end{aligned}$
特征点法

$=\left[\begin{array}{ccccc}{\frac{f_{x}}{Z^{\prime}}} & {0} & {-\frac{X^{\prime} f_{x}}{Z^{\prime 2}}} & {-\frac{X^{\prime} Y^{\prime} f_{x}}{Z^{\prime 2}}} & {f_{x}+\frac{X^{\prime 2} f_{x}}{Z^{\prime 2}}} & {-\frac{Y^{\prime} f_{x}}{Z^{\prime}}} \\ {0} & {\frac{f_{y}}{Z^{\prime}}} & {-\frac{Y^{\prime} f_{y}}{Z^{\prime 2}}} & {-f_{y}-\frac{Y^{\prime 2} f_{y}}{Z^{\prime 2}}} & {\frac{X^{\prime} Y^{\prime} f_{y}}{Z^{\prime 2}}} & {\frac{X^{\prime} f_{y}}{Z^{\prime}}}\end{array}\right]$

1.2 卡尔曼滤波与扩展卡尔曼滤波

卡尔曼滤波在「slam十四讲」中有对应内容，由此可以推导出“kalman filter”

现在的问题是如何求解这个最大化问题。对于高斯分布，最大化问题可以变成最小化它的负对数。这一点在「概率统计」中经常用到。

$\frac{\partial J}{\partial x^{T}}=-H^{T} W^{-1}(z-H x)=0 \Rightarrow\left(H^{T} W^{-1} H\right) x=H^{T} W^{-1} z$

实际上我们可以看出，卡尔曼滤波只用到目前时刻向前的先验信息（或者说是观测数据），但是优化问题中往往需要以后时刻的观测数据进行优化计算。并且更新每个时刻的均值（就是估计值）与方差（就是模型的输入误差方差P）。

关于几种状态估计的方式，这里给出了公式与对比，可以借鉴参考。概括如下：

$\tilde{P}_{k}=F_{k-1} \hat{P}_{k-1} F_{k-1}^{T}+Q_{k}^{\prime}$

$\tilde{x}_{k}=f\left(\hat{x}_{k-1}, v_{k}, 0\right)$

$K_{k}=\tilde{P}_{k} G_{k}^{T}\left(G_{k} \tilde{P}_{k} G_{k}^{T}+R_{k}^{\prime}\right)^{-1}$

$\hat{P}_{k}=\left(I-K_{k} G_{k}\right) \tilde{P}_{k}$

$\hat{x}_{k}=\tilde{x}_{k}+K_{k}\left(y_{k}-g\left(\tilde{x}_{k}, 0\right)\right)$

其中，

$F_{k-1}=\left.\frac{\partial f}{\partial x_{k-1}}\right|_{\hat{x}_{k-1}}, G_{k}=\left.\frac{\partial h}{\partial x_{k}}\right|_{\tilde{x}_{k}}$


几个注意的地方：

即使是高斯分布，经过一个非线性变换后也不是高斯分布。EKF只计算均值与协方差，是在用高斯近似这个非线性变换后的结果。（实际中这个近似可能很差）。
系统本身线性化过程中，丢掉了高阶项。
线性化的工作点往往不是输入状态真实的均值，而是一个估计的均值。于是，在这个工作点下计算的F,G，也不是最好的。
在估计非线性输出的均值时，EKF算的是$\mu_y=f(\mu_x)$的形式。这个结果几乎不会是输出分布的真正期望值。协方差也是同理。

那么，怎么克服以上的缺点呢？途径很多，主要看我们想不想维持EKF的假设。如果我们比较乖，希望维持高斯分布假设，可以这样子改：


为了克服第3条工作点的问题，我们以EKF估计的结果为工作点，重新计算一遍EKF，直到这个工作点变化够小。是为迭代EKF（Iterated EKF, IEKF）。

为了克服第4条，我们除了计算$\mu_y=f(\mu_x)$，再计算其他几个精心挑选的采样点，然后用这几个点估计输出的高斯分布。是为Sigma Point KF（SPKF，或UKF）。
如果不那么乖，可以说：我们不要高斯分布假设，凭什么要用高斯去近似一个长得根本不高斯的分布呢？于是问题变为，丢掉高斯假设后，怎么描述输出函数的分布就成了一个问题。一种比较暴力的方式是：用足够多的采样点，来表达输出的分布。这种蒙特卡洛的方式，也就是粒子滤波的思路。
如果再进一步，可以丢弃滤波器思路，说：为什么要用前一个时刻的值来估计下一个时刻呢？我们可以把所有状态看成变量，把运动方程和观测方程看成变量间的约束，构造误差函数，然后最小化这个误差的二次型。这样就会得到非线性优化的方法，在SLAM里就走向图优化那条路上去了。不过，非线性优化也需要对误差函数不断地求梯度，并根据梯度方向迭代，因而局部线性化是不可避免的。
可以看到，在这个过程中，我们逐渐放宽了假设。

相关信息，可以参见这个人的博客。




github上对卡尔曼滤波总结的比较好。

1.3 卡尔曼滤波仿真代码
下面给出框架代码(matlab)。
%% fusion.m
% Parameters ============================================================

% Biases
BIAS1 = +1;
BIAS2 = -1;

% Measurement noise covariances
R1 = 0.64;
R2 = 0.64;

% Process noise covariance
Q = .005;

% State transition model
A = 1;

% Observation model
C1 = 1;
C2 = 1;

% Duration
N = 1000;

% Run ====================================================================

% Generate the signal x as a sine wave
x = 20 + sin(5*linspace(0,1,N)*pi);

% Add some process noise with covariance Q
w = sqrt(Q) * randn(size(x));
x = x + w;

% Compute noisy sensor values
z1 = BIAS1 + x + sqrt(R1) * randn(size(x));
z2 = BIAS2 + x + sqrt(R2) * randn(size(x));

% Run a single-sensor example and plot it
xhat = kalman(z1, A, C1, R1, Q);

% Plot sensed and estimated values
clf
plotsigs(1, z1, xhat, 'Sensor 1')
title('One sensor: signal cleanup')

% Plot estimate and actual values
plotsigsrms('One sensor', 2, x, xhat)

% Run the Kalman filter on fused sensors
xhat = kalman([z1; z2], A, [C1; C2], [R1 0; 0 R2], Q);

% Plot fusion example
plotsigsrms('Two sensors', 3, x, xhat)

%%kalman.m
function xhat = kalman(z, A, C, R, Q)

% Initializtion =====================================================

% Number of sensors
m = size(C, 1);

% Number of state values
n = size(C, 2);

% Number of observations
numobs = size(z, 2);

% Use linear least squares to estimate initial state from initial
% observation
xhat = zeros(n, numobs);
xhat(:,1) = C \ z(:,1);

% Initialize P, I
P = ones(size(A));
I = eye(size(A));

% Kalman Filter =====================================================

for k = 2:numobs
    
    % Predict
    xhat(:,k) = A * xhat(:,k-1);
    P         = A * P * A' + Q;
    
    % Update
    G         = P  * C' / (C * P * C' + R);
    P         = (I - G * C) * P;
    xhat(:,k) = xhat(:,k) + G * (z(:,k) - C * xhat(:,k));
    
end
end

1.4 为角度设计卡尔曼滤波器
若用C++完成卡尔曼滤波的设计，针对陀螺仪的单项的纠正如下：

在博客中给出了相应的解释。

$\boldsymbol{x}_{k}=\boldsymbol{F} x_{k-1}+\boldsymbol{B} u_{k}+w_{k}$

其中：

$\boldsymbol{x}_{k}=\left[\begin{array}{c}{\theta} \\ {\dot{\theta}_{b}}\end{array}\right]_{k}$

预测方程最终可以写成为：

=\left[\begin{array}{c}\theta+\Delta t\left(\dot{\theta}-\dot{\theta}{b}\right) \ \dot{\theta}{b}\end{array}\right]

问题是这里的bias到底是什么？不是特别确定(偏差量直接作为要估计的项？)

另外，我们计算wk的协方差，过程噪声协方差矩阵，在这种情况下是加速度计和偏差状态估计的协方差矩阵。在这种情况下，我们将认为偏置和加速度计的估计值是独立的，因此它实际上等于加速度计和偏置的估计值的方差。最终定义为：

$\boldsymbol{Q}_{k}=\left[\begin{array}{cc}{Q_{\theta}} & {0} \\ {0} & {Q_{\dot{\theta}_{b}}}\end{array}\right] \Delta t$

调试过程如下：

请注意，如果设置较大的值，则状态估计中的噪声会更多。因此，例如，如果估计的角度开始漂移，则必须增加的值$Q _ {\dot{\theta}b} $ 。否则，如果估计趋于缓慢，则说明您对角度的估计过于信任，应尝试减小的值 $ Q\theta $ 以使其更具响应性。


float Kalman::getAngle(float newAngle, float newRate, float dt) {
    // KasBot V2  -  Kalman filter module - http://www.x-firm.com/?page_id=145
    // Modified by Kristian Lauszus
    // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it

    // Discrete Kalman filter time update equations - Time Update ("Predict")
    // Update xhat - Project the state ahead
    /* Step 1 */
    rate = newRate - bias;
    angle += dt * rate;

    // Update estimation error covariance - Project the error covariance ahead
    /* Step 2 */
    P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
    P[0][1] -= dt * P[1][1];
    P[1][0] -= dt * P[1][1];
    P[1][1] += Q_bias * dt;

    // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
    // Calculate Kalman gain - Compute the Kalman gain
    /* Step 4 */
    float S = P[0][0] + R_measure; // Estimate error
    /* Step 5 */
    float K[2]; // Kalman gain - This is a 2x1 vector
    K[0] = P[0][0] / S;
    K[1] = P[1][0] / S;

    // Calculate angle and bias - Update estimate with measurement zk (newAngle)
    /* Step 3 */
    float y = newAngle - angle; // Angle difference
    /* Step 6 */
    angle += K[0] * y;
    bias += K[1] * y;//这里为什么用bias，我也不太明白。

    // Calculate estimation error covariance - Update the error covariance
    /* Step 7 */
    float P00_temp = P[0][0];
    float P01_temp = P[0][1];

    P[0][0] -= K[0] * P00_temp;
    P[0][1] -= K[0] * P01_temp;
    P[1][0] -= K[1] * P00_temp;
    P[1][1] -= K[1] * P01_temp;

    return angle;
};

1.5 多传感器融合
激光雷达与IMU融合
2 导航其他相关内容

**欧拉角**各种转换关系如下：