雅可比矩阵：用来预测模型？还是求取导数？
在向量分析中，雅可比矩阵(也称作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}$

卡尔曼滤波在「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}}$

几个注意的地方：
1. 即使是高斯分布，经过一个非线性变换后也不是高斯分布。EKF只计算均值与协方差，是在用高斯近似这个非线性变换后的结果。（实际中这个近似可能很差）。
2. 系统本身线性化过程中，丢掉了高阶项。
3. 线性化的工作点往往不是输入状态真实的均值，而是一个估计的均值。于是，在这个工作点下计算的F,G，也不是最好的。
4. 在估计非线性输出的均值时，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上对卡尔曼滤波总结的比较好。
下面给出框架代码(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

若用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;
};

室内的导航定位

主要参见与文档「Paper/ 基于UWB IMU融合的室内定位与导航技术研究.pdf」
噪声因其时间不确定性，计算傅里叶变换没有意义，工程上选取噪声的自相关函数进行频域变换，称为功率谱。

如何设计一个优化系统？

对离散系统的误差进行优化设计，主要是将其构造成为最小二乘估计的形式，采用梯度下降或者高斯牛顿法求解优化问题，以得到想要的最大似然估计。下面是一个简单的示例：

page2

多传感器融合
关于雅可比矩阵的计算

直接法
$\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]$

page3

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

chenshiming1995

发布了10 篇原创文章 · 获赞 1 · 访问量 520

私信关注

导航相关

导航

导航与滤波

室内的导航定位

如何设计一个优化系统？

page2

page3

猜你喜欢