云浮东莞网站建设,是做网站好还是做游戏好,wordpress 字典,网站怎么连接网文章目录0.前言1. IMU的误差状态空间方程2. 误差状态观测方程3. 误差状态卡尔曼滤波4. 误差状态卡尔曼滤波方程细节问题0.前言
这里先说一句#xff1a;什么误差状态卡尔曼#xff1f;完全就是在扯淡#xff01;
回想上面我们推导的IMU的误差状态空间方程#xff0c;其实… 文章目录0.前言1. IMU的误差状态空间方程2. 误差状态观测方程3. 误差状态卡尔曼滤波4. 误差状态卡尔曼滤波方程细节问题0.前言
这里先说一句什么误差状态卡尔曼完全就是在扯淡
回想上面我们推导的IMU的误差状态空间方程其实如果后面我们是使用优化的方法而不是使用滤波的方法那么推导完IMU的误差状态空间方程就可以了。因为上面我们已经说了IMU的误差状态空间方程和状态空间方程的协方差矩阵是完全一致的。所以后面如果是使用优化的方法那么就可以使用误差状态空间方程得到的协方差矩阵作为IMU积分值的置信度了。
但是如果我们是要使用滤波的方法呢那么就要使用基于误差状态而非状态的卡尔曼滤波了也就是我们这里说的误差状态卡尔曼滤波。
这里可能有一个疑问既然我们推导的误差状态和状态的协方差矩阵是相等的那直接用普通的那种基于状态变量的卡尔曼滤波不就可以了吗实际上确实是可以的经过后面的公式推导也可以发现误差状态卡尔曼滤波的公式实际上和普通的卡尔曼滤波没有任何区别所以上面第一句才说“误差状态卡尔曼完全就是在扯淡”
但是为什么还要用它呢实际上只有一个原因就是 旋转的原因前面我们说了旋转的状态变量我们选的是 角轴旋转向量但是旋转向量存在一个问题那就是周期性。所以如果我们用的是 状态旋转向量那么在滤波过程中就存在周期性。但是如果我们选择的是 误差状态旋转向量 呢它始终在0附近所以就 不会产生周期性 的问题。因此实际上使用误差状态卡尔曼滤波完全就是因为这个原因而且现在仔细想想实际上误差状态卡尔曼滤波我们 不应该把它理解为一种新的滤波算法比如 KF 和 EKF 就是两种算法虽然非常像它只是在卡尔曼滤波的时候把状态变量从原来的 状态 变成了 误差状态其他并没有任何变化。
1. IMU的误差状态空间方程
见本系列另一篇博客 Kalman Filter in SLAM (4) ——IMU Intergration and State Space Representation (IMU积分和状态空间表示)
2. 误差状态观测方程
用 z\boldsymbol zz 代表 预测观测值也就是我们把IMU预测得到的状态先验状态 x^k\boldsymbol {\hat{x}}_{k}x^k 带入到观测方程中得到的 计算出来的观测值。现在假设我们的先验状态 x^k\boldsymbol {\hat{x}}_{k}x^k 的误差是 δx^k\delta \boldsymbol {\hat{x}}_{k}δx^k。由于它存在误差导致带入观测方程之后得到的 预测观测值 z\boldsymbol zz 产生的误差是 δz\delta \boldsymbol zδz。利用公式37我们带入 真实的状态值 xkx^kδx^k\boldsymbol x_{k} \boldsymbol {\hat{x}}_{k} \delta \boldsymbol {\hat{x}}_{k}xkx^kδx^k 到 函数自变量 xk\boldsymbol x_kxk 中可以得到真实的状态算出的预测观测值 zδz\boldsymbol z \delta \boldsymbol zzδz 的表达式为
zδzh(x^k,0)Hk(x^kδx^k−x^k)Vkvk\boldsymbol z \delta \boldsymbol z \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) \boldsymbol H_{k} (\boldsymbol {\hat{x}}_{k} \delta \boldsymbol {\hat{x}}_{k} - \boldsymbol {\hat{x}}_{k}) \boldsymbol V_{k} \boldsymbol v_{k} zδzh(x^k,0)Hk(x^kδx^k−x^k)Vkvk
注意我们对观测方程进行线性化最主要的目的就是得到系数矩阵 H\boldsymbol HH但是另外一个同样重要的点就是把状态值带入观测方程计算预测观测值的时候我们也必须带入 线性化的方程而不能带入原来的线性化的方程中这样才能保证我们用的是一个模型。否则就变成了系数矩阵用的线性化模型而计算的观测值用的是非线性模型。因此公式39中我们的自变量只有原来公式37中的 xk\boldsymbol x_kxk误差变量 δxk\delta \boldsymbol x_kδxk 并没有带入到函数 h(x^k,0)\boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0)h(x^k,0) 中因为它只是一个线性函数的常数项。
再由公式38我们可以发现zh(x^k,0)\boldsymbol z \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0)zh(x^k,0)把这个结果带入到公式39中我们就可以得到 状态误差 δxk\delta \boldsymbol x_kδxk 和 预测观测值的误差 δz\delta \boldsymbol zδz 的关系即 误差状态观测方程
δzHkδx^kVkvk\delta \boldsymbol z \boldsymbol H_{k} \delta \boldsymbol {\hat{x}}_{k} \boldsymbol V_{k} \boldsymbol v_{k} δzHkδx^kVkvk
可见这个公式的结果跟我们推导的IMU误差状态空间方程的结果也是非常相似的因为 误差观测方程 的系数矩阵和我们的 状态观测方程 的系数矩阵是完全相等的。
3. 误差状态卡尔曼滤波
我们先给出之前已经推导过的IMU的离散 误差状态空间方程误差状态空间方程两边正好把线性化出来的固定函数值抵消了。之前我们也已经证明了误差状态空间方程和状态空间方程的系数矩阵是完全相等的如下所示
δx^kFk−1δxˇk−1Wk−1wk−1\delta \boldsymbol {\hat{x}}_{k} \boldsymbol F_{k-1} \delta \boldsymbol {\check{x}}_{k-1} \boldsymbol W_{k-1} \boldsymbol w_{k-1} δx^kFk−1δxˇk−1Wk−1wk−1
然后是刚才推导过的 误差状态观测方程
δzHkδx^kVkvk\delta \boldsymbol z \boldsymbol H_{k} \delta \boldsymbol {\hat{x}}_{k} \boldsymbol V_{k} \boldsymbol v_{k} δzHkδx^kVkvk
把这两个方程带入卡尔曼滤波的五大公式中我们可以得到以下公式即为误差状态卡尔曼滤波方程。
预测公式注意输入项 Buk−1\boldsymbol B \boldsymbol u_{k-1}Buk−1 是我们准确知道的所以真实状态减名义状态就消去了
δx^kAδxˇk−1P^kAPˇk−1ATQ\begin{gathered} \delta \boldsymbol {\hat{x}}_{k} \boldsymbol A \delta \boldsymbol {\check{x}}_{k-1}\\ \boldsymbol {\hat P}_{k} \boldsymbol A \boldsymbol {\check P}_{k-1} \boldsymbol A^{T} \boldsymbol Q \end{gathered} δx^kAδxˇk−1P^kAPˇk−1ATQ
校正公式注意其中预测观测值 δz\delta \boldsymbol zδz 带入的时候把噪声 Vkvk\boldsymbol V_{k} \boldsymbol v_{k}Vkvk 简化为0了
KkP^kHTHP^kHTRδxˇkδx^kKk(?zm?−δz)δx^kKk(?zm?−Hδx^k)Pˇk(I−KkH)P^k\begin{gathered} \boldsymbol K_{k}\frac{ \boldsymbol {\hat P}_{k} \boldsymbol H^{T}}{ \boldsymbol H \boldsymbol {\hat P}_{k} \boldsymbol H^{T} \boldsymbol R} \\ \delta \boldsymbol {\check {x}}_{k} \delta \boldsymbol {\hat{x}}_{k} \boldsymbol {K}_{k}\left( ? \boldsymbol {z}_m? -\delta \boldsymbol z \right) \delta \boldsymbol {\hat{x}}_{k} \boldsymbol {K}_{k}\left( ? \boldsymbol {z}_m? \boldsymbol - \boldsymbol H \delta \boldsymbol {\hat{x}}_{k}\right) \\ \boldsymbol {\check P}_{k} \left( \boldsymbol I- \boldsymbol K_{k} \boldsymbol H\right) \boldsymbol {\hat P}_{k} \end{gathered} KkHP^kHTRP^kHTδxˇkδx^kKk(?zm?−δz)δx^kKk(?zm?−Hδx^k)Pˇk(I−KkH)P^k
但是带入的时候我们发现了问题原来的状态卡尔曼滤波我们在校正的时候KkK_kKk 乘以的系数是 观测传感器测量值 −-− 带入预测状态到观测方程中得到的 预测观测值。这里换成误差状态后误差的预测观测值我们已经推导出来了就是 Hδx^k\boldsymbol H \delta \boldsymbol {\hat{x}}_{k}Hδx^k。按理说原本状态卡尔曼滤波中的 传感器测量值zm\boldsymbol {z}_mzm 也应该替换成 传感器测量误差值那么测量误差值应该怎么计算呢
其实仔细思考公式44即 δzHkδx^kVkvk\delta \boldsymbol z \boldsymbol H_{k} \delta \boldsymbol {\hat{x}}_{k} \boldsymbol V_{k} \boldsymbol v_{k}δzHkδx^kVkvk这个公式我们在计算什么因为方程中 已经把传感器的测量噪声项的影响 Vkvk\boldsymbol V_{k} \boldsymbol v_{k}Vkvk 建模进去了所以我们计算的是 仅仅由先验状态误差 引起的 观测测量值误差。那么我们上面说的传感器测量值误差应该是什么呢显然就是 传感器的测量噪声 Vkvk\boldsymbol V_{k} \boldsymbol v_{k}Vkvk因为传感器测量误差就是在说如果我传入的 先验状态就是当前的真实状态但是带入观测方程中计算得到的观测测量值 仍然 和传感器的真实测量值之间有差距这个差距就是因为我在带入观测方程中计算的时候由于不知道此时传感器的噪声是多少所以把它简化成了 0从而计算结果和真实的传感器测量不一致。明白了这个道理之后就很容易计算 传感器的测量误差值 了如下所示
δzmzm−zxktzm−(h(x^k,0)Hk(xk−x^k))zm−h(x^k,0)−Hkδx^k\begin{aligned} \delta \boldsymbol z_m \boldsymbol z_m - \boldsymbol z_{\boldsymbol x_{kt}} \\ \boldsymbol z_m - \left(\boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) \boldsymbol H_{k} (\boldsymbol x_{k} - \boldsymbol {\hat{x}}_{k}) \right) \\ \boldsymbol z_m - \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) - \boldsymbol H_{k} \delta \boldsymbol {\hat{x}}_{k} \end{aligned} δzmzm−zxktzm−(h(x^k,0)Hk(xk−x^k))zm−h(x^k,0)−Hkδx^k
注意
1 同理还是之前那一点我们带入真实状态计算观测测量值的时候带入的仍然是线性化之后的观测方程即 zkh(x^k,0)Hk(xk−x^k)Vkvk\boldsymbol z_{k} \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) \boldsymbol H_{k} (\boldsymbol x_{k} - \boldsymbol {\hat{x}}_{k}) \boldsymbol V_{k} \boldsymbol v_{k}zkh(x^k,0)Hk(xk−x^k)Vkvk 中计算。
2 计算真实状态的测量误差值的时候我们不知道噪声所以计算公式中没有 Vkvk\boldsymbol V_{k} \boldsymbol v_{k}Vkvk 这一项。实际上我们上面的公式就是在求噪声就是在求 Vkvk\boldsymbol V_{k} \boldsymbol v_{k}Vkvk 这一项。
因此误差卡尔曼滤波中的校正公式为
δxˇkδx^kKk((zm−h(x^k,0)−Hkδx^k)−Hδx^k)δx^kKk(zm−h(x^k,0)−2Hkδx^k)\begin{aligned} \delta \boldsymbol {\check {x}}_{k} \delta \boldsymbol {\hat{x}}_{k} \boldsymbol {K}_{k}\left( \boldsymbol ( \boldsymbol z_m - \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) - \boldsymbol H_{k} \delta \boldsymbol {\hat{x}}_{k}) - \boldsymbol H \delta \boldsymbol {\hat{x}}_{k}\right) \\ \delta \boldsymbol {\hat{x}}_{k} \boldsymbol {K}_{k}\left( \boldsymbol {z}_m - \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) - 2\boldsymbol H_{k} \delta \boldsymbol {\hat{x}}_{k} \right) \end{aligned} δxˇkδx^kKk((zm−h(x^k,0)−Hkδx^k)−Hδx^k)δx^kKk(zm−h(x^k,0)−2Hkδx^k)
4. 误差状态卡尔曼滤波方程细节问题
1. 状态的更新
每当我们进行一次误差卡尔曼滤波之后即得到了 后验误差 之后都需要把它加到 先验的状态值 上即
xˇkx^kδxˇk\boldsymbol {\check x_k} \boldsymbol {\hat x_k} \delta \boldsymbol {\check {x}}_{k} xˇkx^kδxˇk
此时我们就得到了经过这次卡尔曼滤波之后我们得到的 对状态xk\boldsymbol {x}_kxk 的最优估计所以说此时我们要把后验误差 δxˇk\delta \boldsymbol {\check x_k}δxˇk 置为 0\boldsymbol 00表示此时我们得到的状态就是我们 能够给出的最优估计的状态结果。
2. 先验估计误差的值和初始化问题
1非初始化状态
根据上面状态更新中的操作可以知道每一次假设第kkk次误差状态卡尔曼滤波结束之后我们的后验误差δxˇk\delta \boldsymbol {\check x_k}δxˇk都是0\boldsymbol 00表示这次我们给出的状态估计结果是最优的状态估计结果。
那么当下一次即第k1k1k1次误差迭代卡尔曼滤波来的时候我们首先使用IMU状态方程计算这一次第k1k1k1次的先验误差δx^k1\delta \boldsymbol {\hat x}_{k1}δx^k1如下注意公式中没有过程噪声Wkwk\boldsymbol {W}_{k} \boldsymbol {w}_kWkwk项因为噪声我们始终都是不知道的。 δx^k1Aδxˇk\delta \boldsymbol {\hat{x}}_{k1} \boldsymbol A \delta \boldsymbol {\check{x}}_{k} δx^k1Aδxˇk 这里就会发现由于上一次的后验误差是0所以这一次我们的先验误差计算结果也是0。所以只要不是在第一次进行误差状态卡尔曼滤波那么卡尔曼校正公式都可以化简为 δxˇkKk(zm−h(x^k,0))\delta \boldsymbol {\check {x}}_{k} \boldsymbol {K}_{k}\left ( \boldsymbol z_m - \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0)\right) δxˇkKk(zm−h(x^k,0)) 所以实际上这个公式才是在误差状态卡尔曼滤波中最常见的校正公式
2初始化状态
那么问题来了第一次进行误差状态卡尔曼滤波的时候先验误差x^0\boldsymbol {\hat x_0}x^0如何给定呢这个问题和卡尔曼滤波问题是一样的其实卡尔曼滤波问题也存在初始化问题就是第一次的先验状态给多少这个感觉就是凭工程经验靠调参来解决。所以这个也是r2live中存在一段函数进行ESKF初始化的原因。
3. 另一个角度思考卡尔曼滤波和误差卡尔曼滤波的校正公式
经过上面的公式推导其实我们已经比较清楚为何ESKF和EKF的校正公式为什么看起来相差比较大了二者校正公式
EKF xˇkx^kKk(zm−Hx^k)\boldsymbol {\check {x}}_{k} \boldsymbol {\hat{x}}_{k} \boldsymbol {K}_{k}\left( \boldsymbol {z}_m - \boldsymbol H \boldsymbol {\hat{x}}_{k}\right) xˇkx^kKk(zm−Hx^k)
ESKF
初始化状态δxˇkδx^kKk((zm−h(x^k,0)−Hkδx^k)−Hδx^k)一般状态δxˇkKk(zm−h(x^k,0))初始化状态\delta \boldsymbol {\check {x}}_{k} \delta \boldsymbol {\hat{x}}_{k} \boldsymbol {K}_{k}\left( \boldsymbol ( \boldsymbol z_m - \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) - \boldsymbol H_{k} \delta \boldsymbol {\hat{x}}_{k}) - \boldsymbol H \delta \boldsymbol {\hat{x}}_{k}\right) \\ 一般状态 \delta \boldsymbol {\check {x}}_{k} \boldsymbol {K}_{k}\left ( \boldsymbol z_m - \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0)\right) 初始化状态δxˇkδx^kKk((zm−h(x^k,0)−Hkδx^k)−Hδx^k)一般状态δxˇkKk(zm−h(x^k,0))
但是从数据融合的意义上来想好像又不明白为什么 ESKF 中一般状态下的先验误差为什么是0。其实可以不从数据融合的意义上想而是从 状态调整的角度 这么想我们先由状态方程得到一个先验状态此时我们只有这个先验状态而且它给出的也是我们 从先验方程中能够得到的最优的状态所以如果没有观测方程那么 先验状态的误差就是0因为这是我们能够给出的最优估计结果。但是现在我们有观测方程所以我们就想利用观测方程的观测结果在我们的先验状态的基础上进行一些调整得到更加准确的后验状态而调整的大小就是卡尔曼增益所在的这一项。
4. 思考为什么能直接把误差状态方程带入KF中变成ESKF
从B站DR_CAN推导KF的公式中看求卡尔曼增益的时候利用的是对 后验误差的协防差矩阵的迹就是方差的和求导为0即 让后验误差的方差总和达到最小值。那么同理如果现在状态向量变成了误差状态那推导的时候不就变成对 误差的后验误差的协方差矩阵的迹 求导了吗这又出来一个 误差的误差
实际上从应用来说不需要考虑到推导这一步。因为卡尔曼滤波的方程推导出来五大公式会根据状态方程和观测方程用就可以了。观察我们上面推导出来了误差的状态方程和误差的观测方程就简单的认为 误差 是另一个 新的状态变量y\boldsymbol yy 就可以了然后它就和原来的状态变量没有任何区别就可以直接带入到卡尔曼滤波的方程中了。
