kalman filter
目录
考试题目
初始对准分为哪些？
对准阶段（粗，精），对准轴系（水平，方位），基座运动状态（静，动），对外信息的需求（自主，非自主）
核心参考
非常棒的ipython电子书https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
https://blog.csdn.net/qq_18163961/article/details/52505591
简介
KF 与 EKF (expand kalman filter)扩展卡尔曼滤波：
EKF的基本思想是将非线性系统线性化，然后进行卡尔曼滤波，因此EKF是一种伪非线性的卡尔曼滤波。
实际中一阶EKF应用广泛。但EKF存在一定的局限性：
- 其一是当强非线性时EKF违背局部线性假设，Taylor展开式中被忽略的高阶项带来大的误差时，EKF算法可能会使滤波发散；
- 另外，由于EKF在线性化处理时需要用雅克比(Jacobian)矩阵，其繁琐的计算过程导致该方法实现相对困难。所以，在满足线性系统、高斯白噪声、所有随机变量服从高斯(Gaussian)分布这3个假设条件时，EKF是最小方差准则下的次优滤波器，其性能依赖于局部非线性度。
结论：
predict：
\[\hat{x}_{k|k-1}=\Phi_{k|k-1}\hat{x}_{k-1}
\]
更新协方差矩阵
\[P_{k|k-1}=\Phi_{k|k-1}P_{k-1}\Phi{k|k-1}+\Gamma_{k-1}Q\Gamma_{k-1}^T
\]
update(矫正)
卡尔曼增益
\[K_k=\frac{P_{k|k-1}H_k^T}{H_kP_{k|k-1}H_k^T+R}=P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T+R)^{-1}
\]
估计值：
\[\hat{x}_k=\hat{x}_{k|k-1}+K_k(Z_k-H_k\hat x_{k|k-1})
\]
更新误差协方差阵
\[P_k=P_{k|k-1}[I-K_kH]
\]
参数设计：
初值
\(\hat{x}_0=0\)与\(p_0=\alpha I\)推荐\(\alpha\)设置的大一些，因为只要系统具有可观性，最后就能收敛到较小的数值
协方差矩阵
| 参数 | 含义 | 含义 | 备注 |
|---|---|---|---|
| Pval | 预测误差 | 系统结果与真实结果的偏差 | |
| Qval | 系统噪声 | 系统输出的随机误差 | |
| Rval | 量测噪声 | 系统输入的随机误差 | 根据仪器的测量精度进行设置 |
注意：不同项的协方差是不一致的，需要根据各自的误差分布和量度单位进行设计，否则会导致系统发散。
推导过程
经典推导
就是线性最小方差估计
前备公式
\[\frac{\partial x^Ty}{\partial x}=y^T
\]
\[\frac{\partial xy^T}{\partial x}=y
\]
\[\frac{\partial y^Tx}{\partial x}=y^T
\]
\[\frac{\partial x^TAx}{\partial x}=x^TA^T+x^TA=2x^TA
\]
\[E(xZ^T)-E(x)E(Z^T)=E\{[x-E(Z)][x-E(Z)]^T\}
\]
量测公式
\[Z = HX+V
\]
其中Z为量测值，H为状态转移矩阵，X为状态量，V为量测误差。
对量测值进行最小二乘，即有如下公式：
\[J=E(Z-\hat{Z})^T(Z-\hat{Z})
\]
\[\frac{\partial J}{\partial \hat{x}}=\frac{\partial (Z-H\hat{x})^T(Z-H\hat{x})}{\partial \hat{x}}
\]
\[\frac{\partial J}{\partial \hat{x}}=\frac{\partial (Z^TZ-\hat{x}^TH^TZ-Z^TH\hat{x}+\hat{x}^TH^TH\hat{x})}{\partial \hat{x}}
\]
\[\frac{\partial J}{\partial \hat{x}}=-Z^TH-Z^TH+Z\hat{x}^TH^TH=-2Z^TH+2\hat{x}^TH^TH
\]
令偏导数为0，也即有解：
\[\hat{x}=(H^TH)^{-1}H^TZ
\]
因此偏差为：
\[\tilde{x}=x-\hat{x}
\]
\[\tilde{x}=x-(H^TH)^{-1}H^TZ
\]
\[\tilde{x}=(H^TH)^{-1}H^THx-(H^TH)^{-1}H^TZ
\]
\[\tilde{x}=(H^TH)^{-1}H^T(Hx-Z)
\]
\[\tilde{x}=(H^TH)^{-1}H^T(V)
\]
\[E(\tilde{x}\tilde{x}^T)=E\{[\tilde{x}-E{\tilde{x}}][\tilde{x}-E{\tilde{x}}]^T\}
\]
\[E(\tilde{x}\tilde{x}^T)=E((H^TH)^{-1}H^T(VV^T)H(H^TH)^{-1})
\]
\[E(\tilde{x}\tilde{x}^T)=E((H^TH)^{-1}H^TRH(H^TH)^{-1})
\]
\[\hat{x}=AZ+b
\]
\[\frac{\partial J}{\partial b}=\frac{\partial E[(x-AZ-b)(x-AZ-b)^T]}{\partial b}
\]
\[\frac{\partial J}{\partial b}=-2E(x-AZ-b)
\]
\[b=E(x)-AE(Z)
\]
\[\frac{\partial J}{\partial A}=\frac{\partial E[(x-AZ+b)(x-AZ+b)^T]}{\partial A}
\]
\[\frac{\partial J}{\partial A}=-2E(x-AZ-b)Z^T
\]
\[\frac{\partial J}{\partial A}=-2bE(Z^T)+2AE(ZZ^T)-2E(xZ^T)^2
\]
\[\frac{\partial J}{\partial A}=???
\]
\[E(W_k)=0: cov(W_k,W_j)=E(W_kW_j^T)=Q\delta_{kj}
\]
\[E(V_k)=0: cov(V_k,V_j)=E(V_kV_j^T)=R\delta_{kj}
\]
k-1时刻，由\(Z_1Z_2...Z_{k-1}\)对x的最优线性估计为
\[E(x|Z_1Z_2...Z_{k-1})=\hat{x}_{k-1}
\]
predict:
\[\hat{x}_{k|k-1}=E(x_k|Z_1Z_2...Z_{k-1})
\]
\[\hat{x}_{k|k-1}=E(\phi_{k,k-1}x_{k-1}+\Gamma_{k,k-1}W_{k-1}|Z_1Z_2...Z_{k-1})
\]
\[\hat{x}_{k|k-1}=\phi_{k,k-1}E(x_{k-1}|Z_1Z_2...Z_{k-1})+\Gamma_{k,k-1}E(W_{k-1}|Z_1Z_2...Z_{k-1})
\]
也即得到
\[x_k=\phi_{k,k-1}x_{k-1}+\Gamma_{k,k-1}W_{k-1}
\]
\[\tilde{x}_{k|k-1}=x_k-\hat{x}_{k|k-1}+\Gamma_{k,k-1}W_{k-1}-\phi_{k,k-1}\hat{x}_{k-1}
\]
\[\tilde{x}_{k|k-1}=\phi_{k,k-1}(x_{k-1}-\hat{x}_{k-1})+\Gamma_{k,k-1}W_{k-1}
\]
\[\tilde{x}_{k|k-1}=\phi_{k,k-1}\tilde{x}_{k-1}+\Gamma_{k,k-1}W_{k-1}
\]
上式为一步预测协方差矩阵
\[P_{k|k-1}=E(\tilde{x}_{k|k-1}\tilde{x}_{k|k-1}^T)=\phi_{k,k-1}P_{k-1}\phi_{k,k-1}^{-1}+\Gamma_{k-1}Q_{k|k-1}\Gamma_{k|k-1}^T
\]
\[\hat{Z}_{k|k-1}=H_k\hat{x}_{k|k-1}
\]
\[\hat{Z}_{k|k-1}=Z_k-\hat{z}_{k|k-1}=H_kx_k-H_k\hat{x}_{k|k-1}=H_k(x_k-\hat{x}_{k|k-1})+V=H_k\tilde{x}_{k|k-1}+V
\]
令
\[\hat{x}_k=\hat{x}_{k|k-1}+k_k\tilde{Z}_{k,k-1}
\]
求\(K_k\)
\[J=E(\tilde{x}_k\tilde{x}_k^T)
\]
令其偏导数\(\frac{\partial T}{\partial x}=0\)
\[\tilde{x}_{k|k}=x_k-\tilde{x}_{k|k}
\]
\[\tilde{x}_{k|k}=x_k-(\hat{x}_{k|k-1}+K_k\tilde{Z}_{k,k-1})
\]
\[\tilde{x}_{k|k}=\tilde{x}_{k,k-1}-K_K(H_Kx_K+V_K-H_K\hat{x}_{K|K-1})
\]
测量值
在K-1时刻对x的最优线性估计\(\hat{x}_{k-1}\)为
\[E(x|Z_1Z_2...Z_{k-1})=\hat{x}_{k-1}
\]
\[J=E(\tilde{x}_{k|k},\tilde{x}_{k|k}^T)=P_K
\]
\[P_K=P_{K|K-1}-P_KH_K^T(H_KP_{k|K-1}H_K^T+R_K)^{-1}H_KP_{K|K-1}
\]
\[P_K=(I-K_KH_K)P_{K|K-1}
\]
\[J=E\{[I-K_KH_K)(\tilde{x}_{K|K-1}-K_KV_K][(I-K_KH_K)\tilde{X}_{K|K-1}]\}
\]
正交投影法
内容
卡尔曼滤波是线性最小方差估计
定义：x，Z，分别是n维，m维的随机向量。若有一个与x同纬度的随机向量\(\hat{x}\)满足下面3个条件：
- \(\hat{x}\)可由\(Z\)线性表示
- \(\hat{x}\)是无偏估计
- \((x-\hat{x})\)与\(Z\)正交
| 前提 | 条件 | 结论 |
|---|---|---|
| a,b独立 | a,b一定不相关 | |
| a,b为正态分布时 | a,b不相关 | a,b独立 |
| a,b为正态分布 | E(a) or E(b) == 0 | 独立，正交，不相关三者等价 |
结论
- x在z上的正交投影\(\hat{x}\)是x在z上的线性最小方差估计
证明
\[E(x-\hat{x})=0
\]
也即不相关与正交等价
\[cov(x-\hat{x},Z)=cov(\hat{x},Z)=E(x-\hat{x})-E[x-\hat{x})]E[Z-E(Z)]^T
\]
\[cov(x-\hat{x},Z)=E[x-\hat{x})]E[Z-E(Z)]^T
\]
\[cov(x-\hat{x},Z)=E[x-(E(x)+cov(x,Z)var(Z)^{-1}???)]E[Z-E(Z)]^T
\]
???
2. \(\hat{E}(Ax|Z)=A\hat{E}(x|Z)\)其中\(A\)为常数阵
3. 分配律
\[\hat{E}(AX+BY|Z)=A\hat{E}(x|Z)+B\hat{E}(Y|Z)
\]
- 更新信息定理
\[Z_1^k=[\frac{Z_1^{k-1}}{Z_k}]
\]
\[\hat{E}[x|Z_1^k]=\hat{E}[x|Z_1^{k-1}]+\hat{E}(\tilde{x}_{k|k-1}|\tilde{Z}_{k|k-1})
\]
\[\hat{E}[x|Z_1^k]=\hat{E}[x|Z_1^{k-1}]+\hat{E}(\tilde{x}_{k|k-1}\tilde{Z}_{k|k-1}^T)E(\tilde{x}_{k|k-1}\tilde{Z}_{k|k-1}^T)^{-1}
\]
推导卡尔曼滤波公式
一步预测的状态估计值
\[\hat{x}_{k-1}=\hat{E}(x_{k-1}|Z_1^{k-1})
\]
\[\hat{x}_{k|k-1}=\hat{E}(x_k|Z_1^{k-1})
\]
\[\hat{x}_{k|k-1}=\hat{E}(\Phi_{k|k-1}+\Gamma_{k|k-1}W_{k-1}|Z_1^{k-1})
\]
\[\hat{x}_{k|k-1}=\hat{E}(\Phi_{k|k-1}x_{k-1}|Z_1^{k-1})+\hat{E}(\Gamma_{k|k-1}W_{k-1}|Z_1^{k-1})
\]
\[\hat{x}_{k|k-1}=\Phi_{k|k-1}\hat{E}(x_{k-1}|Z_1^{k-1})+\Gamma_{k|k-1}\hat{E}(W_{k-1}|Z_1^{k-1})
\]
\[\hat{x}_{k|k-1}=\Phi_{k|k-1}\hat{x}_{k-1}
\]
最小方差估计的推导
\[J=E[\tilde{x}\tilde{x}^T]=E(x-\hat{x})(x-\hat{x})^T
\]
令\(J\)取最小值
\[\hat{x}=AZ+b
\]
\[J=E[x-(AZ+b)][x-(AZ+b)]^T
\]
令\(J\)取最小值
\[\frac{\partial J}{\partial A}=-2E(x-AZ-b)=0
\]
也即有
\[b=E(x)-AE(z)①
\]
\[\frac{\partial J}{\partial A}=-2[E(x-AZ-b)Z^T]
\]
\[\frac{\partial J}{\partial A}=2[bE(Z^T)+AE(ZZ^T)-E(xZ^T)]
\]
带入公式①
\[\frac{\partial J}{\partial A}=2[E(x)-AE(Z)]E(Z^T)+2AE(ZZ^T)-2E(xZ^T)
\]
\[\frac{\partial J}{\partial A}=A·var(Z)-cov(zZ)=0
\]
\[A=conv(xZ)var(Z)^{-1}②
\]
带入公式②
\[\hat{x}=E(x)+cov(xZ)var(z)^{-1}[Z-E(Z)]
\]
也即得到\(\hat{x}\)是\(x\)在\(z\)上的正交投影，记为
\[\hat{x}=\hat{E}(x|Z)
\]
