卡尔曼滤波器原理By_DR_CAN 学习笔记

DR_CAN卡尔曼滤波器

Kalman Filter

    • Recursive Algorithm
      • 迭代过程
    • 数学基础
      • 正态分布和6-Sigma
      • Data Fusion
      • Covariance Matrix
      • State Space Representation
        • 离散化推导
      • linearization
        • Taylor Series
        • 2-D
        • Summary
    • Step by Step Derivation of Kalman Gain
        • 矩阵求导公式
    • Prior / Posterior Error Covariance Matrix
    • An 2D example
    • Extended Kalman Filter(EKF)
    • 理解

Kalman Filter
Optimal Recursive Data Processing Algorithm

不确定性:
1、不存在完美的数学模型
2、系统的扰动不可控,也很难建模
3、测量传感器存在误差

Recursive Algorithm

第 K 次测量结果 z k z_k zk
通过样本均值来估计真实值
估计值 x ^ k = 1 k ( z 1 + z 2 + . . . + z k ) = 1 k ( z 1 + z 2 + . . . + z k − 1 ) + 1 k z k = k − 1 k x ^ k − 1 + 1 k z k = x ^ k − 1 + 1 k ( z k − x ^ k − 1 ) \hat x_k = \frac{1}{k}(z_1+z_2+...+z_k) = \frac{1}{k}(z_1+z_2+...+z_{k-1})+\frac{1}{k}z_k = \frac{k-1}{k}\hat x_{k-1}+\frac{1}{k}z_k=\hat x_{k-1}+\frac{1}{k}(z_k-\hat x_{k-1}) x^k=k1(z1+z2+...+zk)=k1(z1+z2+...+zk1)+k1zk=kk1x^k1+k1zk=x^k1+k1(zkx^k1)

k → ∞ , 1 k → 0 , x ^ k = x ^ k − 1 k \to \infty,\frac{1}{k} \to 0,\hat x_k=\hat x_{k-1} k,k10,x^k=x^k1 测量结果不重要
k → 0 , 1 k → ∞ k \to 0,\frac{1}{k} \to \infty k0,k1 测量值 z k z_k zk作用较大

k k = 1 k k_k=\frac{1}{k} kk=k1,其中 k k k_k kk为Kalman Gain
x ^ k = x ^ k − 1 + k k ( z k − x ^ k − 1 ) \hat x_k = \hat x_{k-1}+k_k(z_k-\hat x_{k-1}) x^k=x^k1+kk(zkx^k1)

当前估计值 = 上次估计值 + 系数 *(当前测量值-上次估计值)
估计误差 e E S T e_{EST} eEST
测量误差 e M E A e_{MEA} eMEA
Klaman Gain k k = e E S T k − 1 e E S T k − 1 + e M E A k k_k = \frac{e_{EST_{k-1}}}{e_{EST_{k-1}}+e_{MEA_k}} kk=eESTk1+eMEAkeESTk1

e E S T k − 1 ≫ e M E A k , k k → 1 , x ^ k = z k e_{EST_{k-1}} \gg e_{MEA_k}, k_k\to1, \hat x_k=z_k eESTk1eMEAk,kk1,x^k=zk 估计误差大,估计值取测量值
e E S T k − 1 ≪ e M E A k , k k → 0 , x ^ k = x ^ k − 1 e_{EST_{k-1}} \ll e_{MEA_k}, k_k\to0, \hat x_k=\hat x_{k-1} eESTk1eMEAk,kk0,x^k=x^k1 测量误差大,估计值取上次估计值

迭代过程

STEP1:计算 Kalman Gain k k = e E S T k − 1 e E S T k − 1 + e M E A k k_k = \frac{e_{EST_{k-1}}}{e_{EST_{k-1}}+e_{MEA_k}} kk=eESTk1+eMEAkeESTk1
STEP2:计算 x ^ k = x ^ k − 1 + k k ( z k − x ^ k − 1 ) \hat x_k = \hat x_{k-1} + k_k(z_k-\hat x_{k-1}) x^k=x^k1+kk(zkx^k1)
STEP3:更新 e E S T k = ( 1 − k k ) e E S T k − 1 e_{EST_{k}}=(1-k_k)e_{EST_{k-1}} eESTk=(1kk)eESTk1

数学基础

正态分布和6-Sigma

通过测量获取多组数据,用样本均值代替期望,样本方差代替方差

样本方差 S 2 = 1 n ∑ i = 1 n ( x i − x ^ ) 2 S^2 = \frac{1}{n}\sum_{i=1}^{n}{(x_i-{\hat x})^2} S2=n1i=1n(xix^)2
样本标准差 S = 1 n ∑ i = 1 n ( x i − x ^ ) 2 S= \sqrt{ \frac{1}{n}\sum_{i=1}^{n}{(x_i-{\hat x})^2} } S=n1i=1n(xix^)2

理论正态分布
在这里插入图片描述

实际修正后的对应表格

σ \sigma σ产出百分比瑕疵百分比
393.3 %6.7%
599.977%0.023 %
699.99966%0.00034%

生产设备

每日生产量3 σ \sigma σ5 σ \sigma σ6 σ \sigma σ
2001天13次21天1次4年1次

false dismissal 漏检
将某些不合格视为合格

false alarm 假警报
将某些合格视为不合格

在这里插入图片描述

Data Fusion

Measurement: z 1 = 30 g z_1=30g z1=30g Standard Deviation: σ 1 = 2 g \sigma_1=2g σ1=2g
Measurement: z 2 = 32 g z_2=32g z2=32g Standard Deviation: σ 1 = 4 g \sigma_1=4g σ1=4g
两者均遵从 Natural Distribution

估计真实值 z ^ \hat z z^
z ^ = z 1 + k ( z 2 − z 1 ) , k ∈ [ 0 , 1 ] \hat z=z_1+k(z_2-z_1),k \in[0, 1] z^=z1+k(z2z1)k[0,1]
求 k 使得 σ z ^ \sigma_{\hat z} σz^最小
σ z ^ 2 = V a r ( z 1 + k ( z 2 − z 1 ) ) = V a r [ ( 1 − k ) z 1 + k z 2 ] = V a r [ ( 1 − k ) z 1 ] + V a r ( k z 2 ) = ( 1 − k ) 2 σ 1 2 + k 2 σ 2 2 \sigma_{\hat z}^2=Var(z_1+k(z_2-z_1)) = Var[(1-k)z_1+kz_2]=Var[(1-k)z_1]+Var(kz_2)=(1-k)^2\sigma_1^2+k^2\sigma_2^2 σz^2=Var(z1+k(z2z1))=Var[(1k)z1+kz2]=Var[(1k)z1]+Var(kz2)=(1k)2σ12+k2σ22

d σ z ^ 2 d k = − 2 ( 1 − k ) σ 1 2 + 2 k σ 2 2 = 0 \frac{d\sigma_{\hat z}^2}{dk}=-2(1-k)\sigma_1^2+2k\sigma_2^2=0 dkdσz^2=2(1k)σ12+2kσ22=0
k = σ 1 2 σ 1 2 + σ 2 2 k=\frac{\sigma_1^2}{\sigma_1^2+\sigma_2^2} k=σ12+σ22σ12

将数据带入
k = σ 1 2 σ 1 2 + σ 2 2 = 4 4 + 16 = 0.2 k=\frac{\sigma_1^2}{\sigma_1^2+\sigma_2^2}=\frac{4}{4+16}=0.2 k=σ12+σ22σ12=4+164=0.2
z ^ = 30 + 0.2 ∗ ( 32 − 30 ) = 30.4 \hat z=30+0.2*(32-30)=30.4 z^=30+0.2(3230)=30.4 最优解
σ z ^ 2 = 0. 8 2 ∗ 4 + 0. 2 2 ∗ 16 = 3.2 \sigma_{\hat z}^2=0.8^2*4+0.2^2*16=3.2 σz^2=0.824+0.2216=3.2

Covariance Matrix

将方差、协方差在一个矩阵中表现出来,体现变量间的联动关系

球员身高(x)体重(y)年龄(z)
瓦尔迪1797433
奥巴梅扬1878031
萨拉赫1757128
平均180.37530.7

方差 σ x 2 = 1 3 [ ( 179 − 180.3 ) 2 + ( 187 − 180.3 ) 2 + ( 175 − 180.3 ) 2 ] = 24.89 \sigma_x^2=\frac{1}{3}[(179-180.3)^2+(187-180.3)^2+(175-180.3)^2]=24.89 σx2=31[(179180.3)2+(187180.3)2+(175180.3)2]=24.89
σ y 2 = 14 \sigma_y^2 = 14 σy2=14
σ z 2 = 4.22 \sigma_z^2=4.22 σz2=4.22

协方差 σ x σ y = 1 3 [ ( 179 − 180.3 ) ( 74 − 75 ) + ( 187 − 180.3 ) ( 80 − 75 ) + ( 175 − 180.3 ) ( 71 − 75 ) ] = 18.7 = σ y σ x \sigma_x \sigma_y=\frac{1}{3}[(179-180.3)(74-75)+(187-180.3)(80-75)+(175-180.3)(71-75)]=18.7=\sigma_y\sigma_x σxσy=31[(179180.3)(7475)+(187180.3)(8075)+(175180.3)(7175)]=18.7=σyσx
σ x σ z = 4.4 = σ z σ x \sigma_x\sigma_z=4.4=\sigma_z\sigma_x σxσz=4.4=σzσx
σ y σ z = 3.3 = σ z σ y \sigma_y\sigma_z=3.3=\sigma_z\sigma_y σyσz=3.3=σzσy

Covariance matrix P = [ σ x 2 σ x σ y σ x σ z σ y σ x σ y 2 σ y σ z σ z σ x σ z σ y σ z 2 ] P= \left[ \begin{matrix} \sigma_x^2 & \sigma_x \sigma_y & \sigma_x \sigma_z \\ \sigma_y \sigma_x & \sigma_y^2 & \sigma_y \sigma_z \\ \sigma_z\sigma_x & \sigma_z\sigma_y & \sigma_z^2 \end{matrix} \right] P= σx2σyσxσzσxσxσyσy2σzσyσxσzσyσzσz2

过度矩阵 a = [ x 1 y 1 z 1 x 2 y 2 z 2 x 3 y 3 z 3 ] − 1 3 [ 1 1 1 1 1 1 1 1 1 ] [ x 1 y 1 z 1 x 2 y 2 z 2 x 3 y 3 z 3 ] 过度矩阵 a=\left[\begin{matrix} x_1 & y_1 & z_1 \\ x_2 & y_2 & z_2 \\ x_3 & y_3 & z_3 \end{matrix}\right] - \frac{1}{3} \left[\begin{matrix} 1 & 1 & 1 \\ 1 & 1 & 1 \\ 1 & 1 & 1 \end{matrix}\right] \left[\begin{matrix} x_1 & y_1 & z_1 \\ x_2 & y_2 & z_2 \\ x_3 & y_3 & z_3 \end{matrix}\right] 过度矩阵a= x1x2x3y1y2y3z1z2z3 31 111111111 x1x2x3y1y2y3z1z2z3

P = 1 3 a T a P=\frac{1}{3}a^Ta P=31aTa

σ x σ y \sigma_x \sigma_y σxσy表示协方差,不是两个标准差相乘

State Space Representation

在这里插入图片描述
m x ¨ = F − k x − b x ˙ m \ddot x=F-kx-b\dot x mx¨=Fkxbx˙
F 为 Input,记为u
m x ¨ = u − k x − b x ˙ m \ddot x=u-kx-b\dot x mx¨=ukxbx˙

取 State Variable
x 1 = x x_1=x x1=x
x 2 = x ˙ x_2=\dot x x2=x˙

x ˙ 1 = x ˙ = x 2 \dot x_1 = \dot x = x_2 x˙1=x˙=x2
x ˙ 2 = x ¨ = 1 m u − k m x − b m x ˙ = 1 m u − k m x 1 − b m x 2 \dot x_2 = \ddot x=\frac{1}{m}u-\frac{k}{m}x-\frac{b}{m}\dot x=\frac{1}{m}u-\frac{k}{m}x_1-\frac{b}{m}x_2 x˙2=x¨=m1umkxmbx˙=m1umkx1mbx2

( x ˙ 1 x ˙ 2 ) = ( 0 1 − k m − b m ) ( x 1 x 2 ) + ( 0 1 m ) u \left(\begin{matrix} \dot x_1 \\ \dot x_2 \end{matrix}\right) = \left(\begin{matrix} 0 & 1 \\ -\frac{k}{m} & -\frac{b}{m} \end{matrix}\right) \left(\begin{matrix} x_1 \\ x_2 \end{matrix}\right)+ \left(\begin{matrix} 0 \\ \frac{1}{m} \end{matrix}\right)u (x˙1x˙2)=(0mk1mb)(x1x2)+(0m1)u

记作 随时间变化 X ˙ ( t ) = A X ( t ) + B U ( t ) \dot X(t) = AX(t) + BU(t) X˙(t)=AX(t)+BU(t)

测量量Measurement
z 1 = x = x 1 位置 z_1=x=x_1 位置 z1=x=x1位置
z 2 = x ˙ = x 2 速度 z_2=\dot x=x_2 速度 z2=x˙=x2速度

( z 1 z 2 ) = ( 1 0 0 1 ) ( x 1 x 2 ) \left(\begin{matrix} z_1 \\ z_2 \end{matrix}\right) = \left(\begin{matrix} 1 & 0 \\ 0 & 1 \end{matrix}\right) \left(\begin{matrix} x_1 \\ x_2 \end{matrix}\right) (z1z2)=(1001)(x1x2)

记作 Z(t) = HX(t)

整个过程充满不确定性,故状态空间方程离散化表达式为
计算值/估计值 X k = A X k − 1 + B U k + W k − 1 X_k = AX_{k-1}+BU_k+W_{k-1} Xk=AXk1+BUk+Wk1
测量值 Z k = H X k + V k Z_k=HX_k + V_k Zk=HXk+Vk

W k − 1 W_{k-1} Wk1 记作过程噪音 Process Noise
V k V_k Vk 记作测量噪音 Measurement Noise

可通过前面介绍的数据融合,将估计值与测量值融合,得到更加可信赖的目标估计值

离散化推导

通过欧拉法(前向差分)将状态空间方程离散化
X ˙ = X k + 1 − X k T = A X k + B U k \dot X = \frac{X_{k+1}-X_{k}}{T}=AX_k+BU_k X˙=TXk+1Xk=AXk+BUk
X k + 1 = ( T A + I ) X k + T B U k X_{k+1} = (TA+I)X_{k}+TBU_{k} Xk+1=(TA+I)Xk+TBUk
记作 X k + 1 = Φ X k + G U k X_{k+1} = \Phi X_{k} + GU_{k} Xk+1=ΦXk+GUk

X k = Φ X k − 1 + G U k − 1 + W k − 1 X_k = \Phi X_{k-1} + GU_{k-1}+W_{k-1} Xk=ΦXk1+GUk1+Wk1

linearization

Taylor Series

线性系统 linear system ⇔ \Leftrightarrow 叠加原理 superposition

x ˙ = f ( x ) \dot x =f(x) x˙=f(x)
x 1 , x 2 x_1, x_2 x1,x2是解
x 3 = k 1 x 1 + k 2 x 2 ( k 1 , k 2 为 c o n s t a n t ) x_3=k_1x_1+k_2x_2(k_1,k_2为constant) x3=k1x1+k2x2(k1,k2constant)
x 3 x_3 x3是解

线性化:Taylor Series
是在某一点附近的线性化,而不是全局线性化

f ( x ) = f ( x 0 ) + f ′ ( x 0 ) 1 ! ( x − x 0 ) + f ′ ′ ( x 0 ) 2 ! ( x − x 0 ) 2 + . . . + f ( n ) ( x 0 ) n ! ( x − x 0 ) n f(x)=f(x_0)+\frac{f'(x_0)}{1!}(x-x_0)+\frac{f''(x_0)}{2!}(x-x_0)^2+...+\frac{f^{(n)}(x_0)}{n!}(x-x_0)^n f(x)=f(x0)+1!f(x0)(xx0)+2!f′′(x0)(xx0)2+...+n!f(n)(x0)(xx0)n
x − x 0 → 0 x-x_0 \rightarrow 0 xx00,则 ( x − x 0 ) 2 → 0 (x-x_0)^2 \rightarrow 0 (xx0)20 ( x − x 0 ) n → 0 (x-x_0)^n \rightarrow 0 (xx0)n0
f ( x ) = f ( x 0 ) + f ′ ( x 0 ) ( x − x 0 ) f(x) = f(x_0) + f'(x_0)(x-x_0) f(x)=f(x0)+f(x0)(xx0) f ( x ) = k 2 x + b f(x) = k_2x+b f(x)=k2x+b

平衡点
x ¨ + x ˙ + 1 x = 1 \ddot x + \dot x + \frac{1}{x} =1 x¨+x˙+x1=1 在平衡点(Fixed Point)附近线性化
x ¨ = x ˙ = 0 \ddot x=\dot x=0 x¨=x˙=0 则平衡点 x 0 = 1 则平衡点x_0=1 则平衡点x0=1
around x 0 : x δ = x 0 + x d x_0:x_\delta = x_0+x_d x0xδ=x0+xd
x ¨ δ + x ˙ δ + 1 x δ = 1 \ddot x_\delta+\dot x_\delta+\frac{1}{x_\delta}=1 x¨δ+x˙δ+xδ1=1
1 x δ 的线性化 1 x δ = 1 x 0 − 1 x 0 2 ( x δ − x 0 ) = 1 − x d \frac{1}{x_\delta}的线性化\frac{1}{x_\delta}=\frac{1}{x_0}-\frac{1}{x_0^2}(x_\delta-x_0)=1-x_d xδ1的线性化xδ1=x01x021(xδx0)=1xd

x ¨ d + x ˙ d + 1 − x d = 1 \ddot x_d+\dot x_d+1-x_d=1 x¨d+x˙d+1xd=1
x ¨ d + x ˙ d − x d = 0 \ddot x_d+\dot x_d-x_d=0 x¨d+x˙dxd=0

2-D

x ˙ 1 = f 1 ( x 1 , x 2 ) \dot x_1=f_1(x_1,x_2) x˙1=f1(x1,x2)
x ˙ 2 = f 2 ( x 1 , x 2 ) \dot x_2=f_2(x_1,x_2) x˙2=f2(x1,x2)

[ x ˙ 1 d x ˙ 2 d ] = [ ∂ f 1 ∂ x 1 ∂ f 1 ∂ x 2 ∂ f 2 ∂ x 1 ∂ f 2 ∂ x 2 ] ∣ x = x 0 [ x 1 d x 2 d ] \left[\begin{matrix}\dot x_{1d} \\ \dot x_{2d}\end{matrix}\right]= \left[\begin{matrix}\frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2}\\ \frac{\partial f_2}{\partial x_1} & \frac{\partial f_2}{\partial x_2} \end{matrix}\right]_{|x=x_0} \left[\begin{matrix} x_{1d} \\ x_{2d}\end{matrix}\right] [x˙1dx˙2d]=[x1f1x1f2x2f1x2f2]x=x0[x1dx2d]

x ¨ + x ˙ + 1 x = 1 \ddot x + \dot x + \frac{1}{x} =1 x¨+x˙+x1=1
Let x 1 = x , x 2 = x ˙ x_1=x,x_2=\dot x x1=xx2=x˙
x ˙ 1 = x ˙ = x 2 x ˙ 2 = x ¨ = − x ˙ − 1 x + 1 \dot x_1=\dot x=x_2\\ \dot x_2=\ddot x=-\dot x-\frac{1}{x}+1 x˙1=x˙=x2x˙2=x¨=x˙x1+1

x ˙ 1 = x ˙ 2 = 0 \dot x_1=\dot x_2=0 x˙1=x˙2=0,平衡点 x 10 = 1 , x 20 = 0 x_{10}=1, x_{20}=0 x10=1,x20=0
[ x ˙ 1 d x ˙ 2 d ] = [ 0 1 1 − 1 ] [ x 1 d x 2 d ] \left[\begin{matrix}\dot x_{1d} \\ \dot x_{2d}\end{matrix}\right]= \left[\begin{matrix}0 & 1 \\ 1 & -1 \end{matrix}\right] \left[\begin{matrix} x_{1d} \\ x_{2d}\end{matrix}\right] [x˙1dx˙2d]=[0111][x1dx2d]


x ˙ 1 d = x 2 d x ˙ 2 d = x 1 d − x 2 d x ¨ d = x d − x ˙ d ⇒ x ¨ d + x ˙ d − x d = 0 \dot x_{1d}=x_{2d}\\ \dot x_{2d}=x_{1d}-x_{2d}\\ \ddot x_d=x_d-\dot x_d \Rightarrow \ddot x_d + \dot x_d - x_d=0 x˙1d=x2dx˙2d=x1dx2dx¨d=xdx˙dx¨d+x˙dxd=0

Summary

1-D
f ( x ) = f ( x 0 ) + f ′ ( x 0 ) ( x − x 0 ) , x − x 0 → 0 f(x)=f(x_0)+f'(x_0)(x-x_0), x-x_0 \rightarrow 0 f(x)=f(x0)+f(x0)(xx0),xx00

2-D
[ x ˙ 1 d x ˙ 2 d ] = [ ∂ f 1 ∂ x 1 ∂ f 1 ∂ x 2 ∂ f 2 ∂ x 1 ∂ f 2 ∂ x 2 ] ∣ x = x 0 → F i x e d P o i n t [ x 1 d x 2 d ] \left[\begin{matrix}\dot x_{1d} \\ \dot x_{2d}\end{matrix}\right]= \left[\begin{matrix}\frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2}\\ \frac{\partial f_2}{\partial x_1} & \frac{\partial f_2}{\partial x_2} \end{matrix}\right]_{|x=x_0 \rightarrow Fixed \ Point} \left[\begin{matrix} x_{1d} \\ x_{2d}\end{matrix}\right] [x˙1dx˙2d]=[x1f1x1f2x2f1x2f2]x=x0Fixed Point[x1dx2d]

Step by Step Derivation of Kalman Gain

X k = A X k − 1 + B U k − 1 + W k − 1 Z k = H X k + V k X_k = AX_{k-1} + BU_{k-1} + W_{k-1}\\ Z_k=HX_k+V_k Xk=AXk1+BUk1+Wk1Zk=HXk+Vk

A为状态矩阵 B为控制矩阵

P(W) ~ N(0, Q) ,W为过程噪声
Q 为协方差矩阵, Q = E [ W W T ] Q=E[WW^T] Q=E[WWT]

P(v) ~ N(0, R),V为测量噪声
R 为协方差矩阵, R = E [ V V T ] R=E[VV^T] R=E[VVT]

先验估计值 X ^ k − = A X k − 1 + B U k − 1 \hat X_k^- = AX_{k-1}+BU_{k-1} X^k=AXk1+BUk1
测量值预测值 Z k = H X k ⟹ X ^ k M E A = H − 1 Z k Z_k=HX_k \Longrightarrow \hat X_{kMEA}=H^{-1}Z_k Zk=HXkX^kMEA=H1Zk

X ^ k = X ^ k − + G ( H − 1 Z k − X ^ k − ) , G ∈ [ 0 , 1 ] \hat X_k = \hat X_k^- + G(H^{-1}Z_k-\hat X_k^{-}), G\in[0, 1] X^k=X^k+G(H1ZkX^k),G[0,1]
G = K k H G=K_kH G=KkH,带入上式可得:
X ^ k = X ^ k − + K k ( Z k − H X ^ k − ) , K k ∈ [ 0 , H − 1 ] \hat X_k = \hat X_k^{-} + K_k(Z_k-H\hat X_k^-), K_k \in [0, H^{-1}] X^k=X^k+Kk(ZkHX^k),Kk[0,H1]

目标: 寻找 K k ,使得 X ^ k → X k ,即使得 V a r ( X ^ k ) 最小 K_k,使得\hat X_k \to X_k,即使得Var(\hat X_k)最小 Kk,使得X^kXk,即使得Var(X^k)最小

e k = X k − X ^ k e_k=X_k - \hat X_k ek=XkX^k
P ( e k ) P(e_k) P(ek) ~ N(0, P)
P 为误差协方差矩阵, P = E ( e e T ) P=E(ee^T) P=E(eeT)
t r ( P ) 最小时,方差最小 tr(P)最小时,方差最小 tr(P)最小时,方差最小

P = E ( e e T ) = E [ ( X k − X ^ k ) ( X k − X ^ k ) T ] P=E(ee^T)=E[(X_k-\hat X_k)(X_k-\hat X_k)^T] P=E(eeT)=E[(XkX^k)(XkX^k)T]

X k − X ^ k = X k − [ X ^ k − + K k ( Z k − H X ^ k − ) ] = X k − X ^ k − − K k Z k + K k H X ^ k − = X k − X ^ k − − K k ( H X k + V k ) + K k H X ^ k − = ( X k − X ^ k − ) − K k H ( X k − X ^ k − ) − K k V k = ( I − K k H ) ( X k − X ^ k − ) − K k V k = ( I − K k H ) e k − − K k V k X_k-\hat X_k=X_k - [\hat X_k^- + K_k(Z_k-H\hat X_k^-)]\\ =X_k - \hat X_k^- - K_kZ_k + K_kH\hat X_k^-\\ =X_k - \hat X_k^- - K_k(HX_k+V_k) + K_kH\hat X_k^-\\ =(X_k-\hat X_k^-) - K_kH(X_k-\hat X_k^-)-K_kV_k\\ =(I-K_kH)(X_k-\hat X_k^-)-K_kV_k\\ =(I-K^kH)e_k^- -K_kV_k XkX^k=Xk[X^k+Kk(ZkHX^k)]=XkX^kKkZk+KkHX^k=XkX^kKk(HXk+Vk)+KkHX^k=(XkX^k)KkH(XkX^k)KkVk=IKkH)(XkX^k)KkVk=(IKkH)ekKkVk
其中 e k − 称为先验误差 e_k^-称为先验误差 ek称为先验误差

E [ ( I − K k H ) e k − − K k V k ] [ ( I − K k H ) e k − − K k V k ] T = E [ ( I − K k H ) e k − − K k V k ] [ e k − T ( I − K k H ) T − V k T K k T ] = E [ ( I − K k H ) e k − e k − T ( I − K k H ) T − ( I − K k H ) e k − V k T K k T − K k V k e k − T ( I − K k H ) T + K k V k V k T K k T ] E[(I-K_kH)e_k^--K_kV_k][(I-K_kH)e_k^--K_kV_k]^T\\ =E[(I-K_kH)e_k^--K_kV_k][e_k^{-T}(I-K_kH)^T-V_k^TK_k^T]\\ =E[(I-K_kH)e_k^-e_k^{-T}(I-K_kH)^T-(I-K_kH)e_k^-V_k^TK_k^T-K_kV_ke_k^{-T}(I-K_kH)^T+K_kV_kV_k^TK_k^T] E[(IKkH)ekKkVk][(IKkH)ekKkVk]T=E[(IKkH)ekKkVk][ekT(IKkH)TVkTKkT]=E[(IKkH)ekekT(IKkH)T(IKkH)ekVkTKkTKkVkekT(IKkH)T+KkVkVkTKkT]

E ( I − K k H ) e k − V k T K k T = ( I − K k H ) E ( e k − V k T ) K k T = ( I − K k H ) E ( e k − ) E ( V k T ) K k T = 0 E(I-K_kH)e_k^-V_k^TK_k^T\\ =(I-K_kH)E(e_k^-V_k^T)K_k^T\\ =(I-K_kH)E(e_k^-)E(V_k^T)K_k^T=0 E(IKkH)ekVkTKkT=(IKkH)E(ekVkT)KkT=(IKkH)E(ek)E(VkT)KkT=0

P k = ( I − K k H ) E ( e k − e k − T ) ( I − K k H ) T + K k E ( V k V k T ) K k T = ( I − K k H ) P k − ( I − K k H ) T + K k R K k T = ( P k − − K k H P k − ) ( I − H T K k T ) + K k R K k T = P k − − P k − H T K k T − K k H P k − + K k H P k − H T K k T + K k R K k T P_k=(I-K_kH)E(e_k^-e_k^{-T})(I-K_kH)^T+K_kE(V_kV_k^T)K_k^T\\ =(I-K_kH)P_k^-(I-K_kH)^T+K_kRK_k^T\\ =(P_k^--K_kHP_k^-)(I-H^TK_k^T)+K_kRK_k^T\\ = P_k^--P_k^-H^TK_k^T-K_kHP_k^-+K_kHP_k^-H^TK_k^T+K_kRK_k^T Pk=(IKkH)E(ekekT)(IKkH)T+KkE(VkVkT)KkT=(IKkH)Pk(IKkH)T+KkRKkT=(PkKkHPk)(IHTKkT)+KkRKkT=PkPkHTKkTKkHPk+KkHPkHTKkT+KkRKkT

t r ( P k ) = t r ( P k − ) − 2 t r ( K k H P k − ) + t r ( K k H P k − H T K k T ) + t r ( K k R K k T ) tr(P_k)=tr(P_k^-)-2tr(K_kHP_k^-)+tr(K_kHP_k^-H^TK_k^T)+tr(K_kRK_k^T) tr(Pk)=tr(Pk)2tr(KkHPk)+tr(KkHPkHTKkT)+tr(KkRKkT)

矩阵求导公式

d t r ( A B ) d A = B T \frac{dtr(AB)}{dA}=B^T dAdtr(AB)=BT
d t r ( A B A T ) d A = 2 A B \frac{dtr(ABA^T)}{dA}=2AB dAdtr(ABAT)=2AB

d t r ( P k ) d K k = 0 − 2 ( H P k − ) T + 2 K k H P k − H T + 2 K k R = − P k − H T + K k ( H P k − H T + R ) = 0 \frac{dtr(P_k)}{dK_k}=0-2(HP_k^-)^T+2K_kHP_k^-H^T+2K_kR\\ =-P_k^-H^T+K_k(HP_k^-H^T+R)=0 dKkdtr(Pk)=02(HPk)T+2KkHPkHT+2KkR=PkHT+Kk(HPkHT+R)=0

K k = P k − H T H P k − H T + R K_k=\frac{P_k^-H^T}{HP_k^-H^T+R} Kk=HPkHT+RPkHT
R R R 为测量噪声协方差矩阵
P k − P_k^- Pk 为先验误差协方差矩阵

Prior / Posterior Error Covariance Matrix

X k = A X k − 1 + B U k − 1 + W k − 1 , W X_k = AX_{k-1}+BU_{k-1}+W_{k-1},W Xk=AXk1+BUk1+Wk1W~ P ( 0 , Q ) P(0, Q) P(0,Q)
Z k = H W k + V k , V Z_k = HW_k+V_k,V Zk=HWk+VkV~ P ( 0 , R ) P(0, R) P(0,R)

先验估计
X ^ k − = A X ^ k − 1 + B U k − 1 \hat X_k^-=A\hat X_{k-1}+BU_{k-1} X^k=AX^k1+BUk1

后验估计
X ^ k = X ^ k − + K k ( Z k − H X ^ k − ) \hat X_k=\hat X_k^-+K_k(Z_k-H\hat X_k^-) X^k=X^k+Kk(ZkHX^k)

卡尔曼增益
K k = P k − H T H P k − H T + R K_k=\frac{P_k^-H^T}{HP_k^-H^T+R} Kk=HPkHT+RPkHT

P k − P_k^- Pk

误差 e k = X k − X ^ k e_k=X_k-\hat X_k ek=XkX^k
先验误差 e k − = X k − X ^ k − = A X k − 1 + B U k − 1 + W k − 1 − A X ^ k − 1 − B U k − 1 = A ( X k − 1 − X ^ k − 1 ) + W k − 1 = A e k − 1 + W k − 1 e_k^-=X_k-\hat X_k^-=AX_{k-1}+BU_{k-1}+W_{k-1}-A\hat X_{k-1}-BU_{k-1}\\ =A(X_{k-1}-\hat X_{k-1})+W_{k-1}=Ae_{k-1}+W_{k-1} ek=XkX^k=AXk1+BUk1+Wk1AX^k1BUk1=A(Xk1X^k1)+Wk1=Aek1+Wk1

P k − = E [ e k − e k − T ] = E [ ( A e k − 1 + W k − 1 ) ( e k − 1 T A T + W k − 1 T ) ] = E [ A e k − 1 e k − 1 T A T + A e k − 1 W k − 1 T + W k − 1 e k − 1 T A T + W k − 1 W k − 1 T ] = A E ( e k − 1 e k − 1 T ) A T + E ( W k − 1 W k − 1 T ) = A P k − 1 A T + Q P_k^-=E[e_k^-e_k^{-T}]\\ =E[(Ae_{k-1}+W_{k-1})(e^T_{k-1}A^T+W_{k-1}^T)]\\ =E[Ae_{k-1}e_{k-1}^TA^T+Ae_{k-1}W_{k-1}^T+W_{k-1}e_{k-1}^TA^T+W_{k-1}W_{k-1}^T]\\ =AE(e_{k-1}e_{k-1}^T)A^T+E(W_{k-1}W_{k-1}^T)\\ =AP_{k-1}A^T+Q Pk=E[ekekT]=E[(Aek1+Wk1)(ek1TAT+Wk1T)]=E[Aek1ek1TAT+Aek1Wk1T+Wk1ek1TAT+Wk1Wk1T]=AE(ek1ek1T)AT+E(Wk1Wk1T)=APk1AT+Q

使用卡尔曼滤波器 估计 状态变量的值(最终五个式子)
  • 预测
    – 先验: X ^ k − = A X ^ k − 1 + B U k − 1 \hat X_k^-=A\hat X_{k-1}+BU_{k-1} X^k=AX^k1+BUk1
    – 先验误差协方差矩阵: P k − = A P k − 1 A T + Q P_k^-=AP_{k-1}A^T+Q Pk=APk1AT+Q

  • 校正
    – 卡尔曼增益: K k = P k − H T H P k − H T + R K_k=\frac{P_k^-H^T}{HP_k^-H^T+R} Kk=HPkHT+RPkHT
    – 后验估计: X ^ k = X ^ k − + K k ( Z k − H X ^ k − ) \hat X_k=\hat X_k^-+K_k(Z_k-H\hat X_k^-) X^k=X^k+Kk(ZkHX^k)
    – 更新误差协方差
    P k = P k − − P k − H T K k T − K k H P k − + K k H P k − H T K k T + K k R K k T = P k − − P k − H T K k T − K k H P k − + K k ( H P k − H T + R ) K k T = P k − − P k − H T K k T − K k H P k − + P k − H T K k T = P k − − K k H P k − = ( I − K k H ) P k − P_k=P_k^--P_k^-H^TK_k^T-K_kHP_k^-+K_kHP_k^-H^TK_k^T+K_kRK_k^T\\ =P_k^--P_k^-H^TK_k^T-K_kHP_k^-+K_k(HP_k^-H^T+R)K_k^T\\ =P_k^--P_k^-H^TK_k^T-K_kHP_k^-+P_k^-H^TK_k^T\\ =P_k^--K_kHP_k^-\\ =(I-K_kH)P_k^- Pk=PkPkHTKkTKkHPk+KkHPkHTKkT+KkRKkT=PkPkHTKkTKkHPk+Kk(HPkHT+R)KkT=PkPkHTKkTKkHPk+PkHTKkT=PkKkHPk=(IKkH)Pk

An 2D example

excel 矩阵处理函数
矩阵相乘 mmul()
转换 transpose()
取逆 minverse()

全选F2
Ctrl+Shift+Enter

该案例需要赋初始值变量
状态变量初始值 x 1 , 0 , x 2 , 0 后验估计值初始值 x ^ 1 , 0 , x ^ 2 , 0 误差协方差矩阵 P 0 状态变量初始值 x_{1,0}, x_{2,0}\\ 后验估计值初始值 \hat x_{1,0}, \hat x_{2,0}\\ 误差协方差矩阵 P_0 状态变量初始值x1,0,x2,0后验估计值初始值x^1,0,x^2,0误差协方差矩阵P0

Extended Kalman Filter(EKF)

理解

卡尔曼增益K:负责融合估计值与测量值,谁方差小,就更相信谁一些。

状态估计协方差矩阵P:初始状态与过程噪声有关,并且由于每次K的迭代,状态估计协方差矩阵也在迭代(因为使用了上一次的结果作为下一次的初始状态,上一次的结果来自于卡尔曼增益K对观测与估计的融合,状态估计协方差会减小)

过程噪声协方差矩阵Q:来自于世界中的不确定性,这个值怎么选,我也不太清楚,不知道如何度量世界中的不确定性。

测量误差协方差矩阵R:来自于传感器误差(我猜是可以通过试验获得,比如测量100次,然后记录数据)

其余的就是要给一个目标的初始状态,然后根据观测,不断地更新估计,得到一个稳定的K。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/641871.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

如何在Docker上运行Redis

环境: 1.windows系统下的Docker deckstop 1.Pull Redis镜像 2.运行Redis镜像 此时,Redis已经启动,我们登录IDEA查看下是否连接上了 显示连接成功,证明已经连接上Docker上的Redis了

积分梳状滤波器CIC原理与实现

CIC(Cascade Intergrator Comb):级联积分梳状滤波器,是由积分器和梳状滤波器级联而得。滤波器系数为1,无需对系数进行存储,只有加法器、积分器和寄存器,资源消耗少,运算速率高&#…

如何基于 ESP32 芯片测试 WiFi 连接距离、获取连接的 AP 信号强度(RSSI)以及 WiFi吞吐测试

测试说明: 测试 WiFi 连接距离,是将 ESP32 作为 WiFi Station 模式来连接路由器,通过在开阔环境下进行拉距来测试。另外,可以通过增大 WiFi TX Power 来增大连接距离。 获取连接的 AP 信号强度,一般可以通过 WiFi 扫描…

Java应用崩溃的排查流程

目录 分析问题 hs_err_pid.log 上周排查了一个java应用的崩溃问题,在这里记录一下。 分析问题 首先是排查到/tmp目录下有很多的core文件,形式类似: core-18238-java-1705462412 1.3 GB 程序崩溃数据 2024-01-17 11:33:44 core-18108…

Leetcode28-合并相似的物品(2363)

1、题目 给你两个二维整数数组 items1 和 items2 ,表示两个物品集合。每个数组 items 有以下特质: items[i] [valuei, weighti] 其中 valuei 表示第 i 件物品的 价值 ,weighti 表示第 i 件物品的 重量 。 items 中每件物品的价值都是 唯一…

语义分割常用评价指标

在图像处理领域中,语义分割是很重要的一个任务。在实际项目开发中,评估模型预测效果以及各指标的含义对于优化模型极为重要。 本文将主要评价指标的计算算法进行了详细说明,并加上注释解释每个指标的含义。这对理解各指标背后的数学原理以及能否在实践中应用或许有…

GPS位置虚拟软件 AnyGo mac激活版

AnyGo for Mac是一款一键将iPhone的GPS位置更改为任何位置的强大软件!使用AnyGo在其iOS或Android设备上改变其GPS位置,并在任何想要的地方显示自己的位置。这对那些需要测试应用程序、游戏或其他依赖于地理位置信息的应用程序的开发人员来说非常有用&…

Python - SnowNLP 情感分析与自定义训练

目录 一.引言 二.SnowNLP 情感分析 1.安装 SnowNLP 2.测试 SnowNLP 三.SnowNLP 自定义训练 1.数据集准备 2.训练与保存 3.模型替换 4.模型测试 5.SnowNLP 原理 ◆ Bayes 公式 ◆ 先验概率 ◆ 后验概率 ◆ 情感模型 四.总结 一.引言 SnowNLP 是一个基于 Python …

Android双指缩放ScaleGestureDetector检测放大因子大图移动到双指中心点ImageView区域中心,Kotlin

Android双指缩放ScaleGestureDetector检测放大因子大图移动到双指中心点ImageView区域中心,Kotlin 在 Android双击图片放大移动图中双击点到ImageView区域中心,Kotlin-CSDN博客 基础上,这次使用ScaleGestureDetector检测两根手指的缩放动作&a…

Python如何叠加两张图片

我这里有如下两张图片,需要把他们叠加在一起,进行查看。这两张图片的大小都是300 300。不拼接在一起就不方便查看。需要把左边的小图,放到右边大图的中间。 一、拼接两个图片的代码 要解决这个问题,你可以使用fromarray()方法将…

JoyRL Actor-Critic算法

策略梯度算法的缺点 这里策略梯度算法特指蒙特卡洛策略梯度算法,即 REINFORCE 算法。 相比于 DQN 之类的基于价值的算法,策略梯度算法有以下优点。 适配连续动作空间。在将策略函数设计的时候我们已经展开过,这里不再赘述。适配随机策略。由…

MATLAB数据处理: 每种样本类型随机抽样

tn5;% 每种类型随机抽样数 indextrain[];% 训练样本序号集 for i1:typenumber index301 find(typemat i); n2length(index301); index302randperm(n2); index401index301(index302(1:tn)); indextrain[indextrain; index401]; end 该代码可以对大样…

java进阶

文章目录 一、Java进阶1.注解(Annotation)a.内置注解b.元注解c.自定义注解 2.对象克隆3. Java设计模式(Java design patterns)a.软件设计模式概念b.建模语言(UML)c.面向对象设计原则d.设计模式 总结面向对象…

项目工程下载与XML配置文件下载:EtherCAT超高速实时运动控制卡XPCIE1032H上位机C#开发(十)

XPCIE1032H功能简介 XPCIE1032H是一款基于PCI Express的EtherCAT总线运动控制卡,可选6-64轴运动控制,支持多路高速数字输入输出,可轻松实现多轴同步控制和高速数据传输。 XPCIE1032H集成了强大的运动控制功能,结合MotionRT7运动…

深度解析Oladance、韶音、南卡开放式耳机:选购指南与天花板级推荐

​随着开放式耳机在日常生活中越来越受欢迎,许多品牌纷纷降低材料品质以迎合大众需求,导致耳机的性能和音质严重下滑。这让消费者在选择优质开放式耳机时感到困惑。作为一名专业的耳机评测人员,我近期对多款热门开放式耳机进行了深入的测评&a…

Leetcode—92.反转链表II【中等】

2023每日刷题(八十一) Leetcode—92.反转链表II 算法思想 实现代码 /*** Definition for singly-linked list.* struct ListNode {* int val;* ListNode *next;* ListNode() : val(0), next(nullptr) {}* ListNode(int x) : val(x), n…

kubernetes Pod 异常排查步骤

kubernetes Pod 异常排查步骤 详细排查图查看容器状态查看容器列表容器未启动成功排查容器启动成功排查pod状态对应原因 详细排查图 查看容器状态 查看容器列表 查看容器列表,最好在后面跟上命名空间,不跟上查询出来是默认的 kubectl get pods -n kubesphere-system单独查看某…

【Spring 篇】深入探讨MyBatis映射文件中的动态SQL

MyBatis,这个名字在Java开发者的世界中犹如一道光芒,照亮着持久层操作的道路。而在MyBatis的映射文件中,动态SQL则是一个让人爱-hate的存在。有时候,你感叹它的灵活性,有时候,你可能会为它的繁琐而头痛。但…

windows 11安装VMware 17 ,VMware安装Ubuntu 20.4

一、下载安装激活VMware 17 下载与激活:Vmware 17 下载地址、最新激活码 2024 _ 注意:安装路径自己选择,路径中尽可能避免中文或空格 二、下载Ubuntu 镜像 下载镜像地址:清华大学开源软件镜像站 点开下载镜像地址,找…

中科星图——Sentinel-2_MSI_L2A数据集

数据名称: Sentinel-2_MSI_L2A 数据来源: Copernicus 时空范围: 2022年10月-2023年1月 空间范围: 全国 数据简介: 哨兵2号(Sentinel-2)卫星是高分辨率多光谱成像卫星,携带一…