基于等效电路与扩展卡尔曼滤波的SOC估算方法_卡尔曼滤波soc算法

等效电路模型

简介

参考：锂电池的二阶RC网络等效电路模型 - 車馬炮的文章 - 知乎 https://zhuanlan.zhihu.com/p/407572989

二阶RC网络等效电路模型

• 锂离子动力电池正负极之间存在电位差，外在表现为电池的电动势能，用电容器模拟；电池电动势能与电池SOC之间存在关系，在模型中用EMF=f(SOC)表达式描述；

• 锂离子动力电池通过锂离子在正负极之间的嵌入、脱嵌和移动来实现电流的输出，在这个电化学反应和离子运动的过程中，必然会产生一定的阻力，因此在建模时，我们使用电池的内阻来模拟。研究表明，在电池工作时，由于形成的机理不同，电池的内阻又可以进一步划分为欧姆内阻和极化内阻：

1. 欧姆内阻是由于离子在运动过程中受到的阻力而产生的；
2. 极化内阻是由于材料本身的一些性质而产生的，可细分为电化学极化内阻和浓差极化内阻 。

• 电化学极化也称活化极化，是由于正负极活性物质发生的电化学反应速率小于电子运动速率引起的极化，响应时间微秒级；
• 浓差极化是由于反应物消耗引起电极表面得不到及时补充（或是某种产物在电极表面积累，不能及时疏散），例如锂离子在电池正极的积累，导致电极电势偏离通电前按总体浓度计算的平均值，响应时间秒级。

在模型中，通过电阻来描述欧姆内阻，通过两个阻容网络（RC 网络）来描述极化内阻。通过和来描述电池工作时的负载电流和电池的端电压。从图中可以看出改进模型通过二极管的单向导电性对欧姆内阻区别充电与放电设置。放电时对应放电欧姆内阻，充电时对应充电欧姆内阻。考虑到电池的开路电压应该包括滞回电压和电动势两部分，所以增加了等效电压源部分：其中 EMF 是电池平衡电势，是滞回电压（历史电流方向会影响的正负号）。

基于等效电路与扩展卡尔曼滤波的SOC估算方法

1. 数据需求

HPPC测试数据：三元锂与磷酸铁锂

基于HPPC测试数据对等效电路模型中的SOC-OCV，内阻，极化电阻和极化电容进行识别，在测试前先对电芯进行恒流恒压满充，然后根据如下步骤进行测试:

2. 方法介绍

2.1 等效电路模型

二阶等效电路的动态方程如下：
U L = U O C V − I R 0 − U p 1 − U p 2 I = U p 1 R p 1 + C p 1 d U p 1 d t = U p 2 R p 2 + C p 2 d U p 2 d t

$$\begin{align} U_L&=U_{OCV}-IR_0-U_{p1}-U_{p2} \\ I&=\frac{U_{p1}}{R_{p1}}+C_{p1}\frac{dU_{p1}}{dt}=\frac{U_{p2}}{R_{p2}}+C_{p2}\frac{dU_{p2}}{dt} \end{align}$$ UL​I​=UOCV​−IR0​−Up1​−Up2​=Rp1​Up1​​+Cp1​dtdUp1​​=Rp2​Up2​​+Cp2​dtdUp2​​​​
求解微分方程$I=\frac{U_p}{R_p}+C_p\frac{d{U}_p}{dt}$有：
$$\begin{array}{rl}{U}_p& =I{R}_p\left(1-e^{-\frac{t}{R_p{C}_p}}\right)+U_p(0)e^{-\frac{t}{R_p{C}_p}}\\ U_L& =U_{OCV}-I{R}_0-\left(I{R}_{p_1}\left(1-e^{-\frac{t}{R_{p_1}{C}_{p_1}}}\right)+U_{p_1}(0)e^{-\frac{t}{R_{p_1}{C}_{p_1}}}\right)-\left(I{R}_{p_2}\left(1-e^{-\frac{t}{R_{p_2}{C}_{p_2}}}\right)+U_{p_2}(0)e^{-\frac{t}{R_{p_2}{C}_{p_2}}}\right)\end{array}$$

2.2 参数识别

2.2.1 SOC-OCV

选择100%，95%，…，0%的测试节点中长时间静置的末端电压作为对应的OCV.

2.2.2 内阻

$$R_0=\frac{\Delta U_{AB}+\Delta U_{DC}}{2I}$$

2.2.3 极化电阻&极化电容

当BC段放电开始时，为零状态响应，有$t=0,U_p(0)=0$：
$$U_L=U_{OCV}-I{R}_0-I{R}_{p_1}\left(1-e^{-\frac{t}{R_{p_1}{C}_{p_1}}}\right)-I{R}_{p_2}\left(1-e^{-\frac{t}{R_{p_2}{C}_{p_2}}}\right)$$
当DE段搁置时，为零输入响应，有$I=0$：
$$U(t)=U_{OCV}-U_{p1}(0)e^{-\frac{t}{R_{p_1}{C}_{p_1}}}-U_{p2}(0)e^{-\frac{t}{R_{p_2}{C}_{p_2}}}$$
用指数拟合的方法可以求出$U_{p1}(0),U_{p2}(0),-\frac{1}{R_{p_1}{C}_{p_1}},-\frac{1}{R_{p_2}{C}_{p_2}}$。因为DE段开始为BC段结尾，因此：
U p 1 ( 0 ) = I R p 1 ( 1 − e − T C R p 1 C p 1 ) U p 2 ( 0 ) = I R p 2 ( 1 − e − T C R p 2 C p 2 )

$$\begin{align} U_{p1}(0)&=IR_{p_1}\left(1-e^{-\frac{T_C}{R_{p_1}C_{p_1}}}\right)\\ U_{p2}(0)&=IR_{p_2}\left(1-e^{-\frac{T_C}{R_{p_2}C_{p_2}}}\right) \end{align}$$ Up1​(0)Up2​(0)​=IRp1​​(1−e−Rp1​​Cp1​​TC​​)=IRp2​​(1−e−Rp2​​Cp2​​TC​​)​​
从而求出具体$R_p，C_p$。对与更高阶的等效电路模型，求得的方法一致，只是参数更多。

2.3 基于扩展卡尔曼滤波的SOC估算

根据等效电路的参数识别有下表

状态变量设置为$X_k={\left(SOC(k),U_{p1}(k),U_{p2}(k)\right)}^T$，电芯最大可用容量C，时间间隔为t，则状态方程为
$$X_k=A{X}_{k-1}+B{I}_k+{\omega }_k$$
其中，${\omega }_k\sim N(0,Q_k)$， A = ( 1 0 0 0 e − t C p 1 R p 1 0 0 0 e − t C p 1 R p 1 ) A=\left(

$$\begin{matrix}1&0&0\\0&e^{-\frac{t}{C_{p1}R_{p1}}}&0\\0&0& e^{-\frac{t}{C_{p1}R_{p1}}}\end{matrix}$$ \right) A= ​100​0e−Cp1​Rp1​t​0​00e−Cp1​Rp1​t​​ ​，$B=\left(\begin{array}{c}-\frac{t\times 100}{C}\\ R_{p1}(1-e^{-\frac{t}{C_{p1}{R}_{p1}}})\\ R_{p2}(1-e^{-\frac{t}{C_{p2}{R}_{p2}}})\end{array}\right)$，矩阵中参数根据$SOC(k)$进行匹配.

因为观测方程为
U k = U O C V ( S O C ( k ) ) − U p 1 ( k ) − U p 2 ( k ) − I k R 0 + v k

$$\begin{align*}U_k&= U_{OCV}\left(SOC(k)\right)-U_{p1}(k)-U_{p2}(k)-I_kR_0+v_k\\ \end{align*}$$ Uk​​=UOCV​(SOC(k))−Up1​(k)−Up2​(k)−Ik​R0​+vk​​
其中，$v_k\sim N(0,R_k)$.

把观测方程进行泰勒展开：
U k = g ( X k ) = U O C V ( S O C ( k ) ) − U p 1 ( k ) − U p 2 ( k ) − I k R 0 + v k U k ≈ g ( X k − 1 ) + g ′ ( X k − 1 ) ( X ( k ) − X ( k − 1 ) ) H = g ′ ( X k − 1 ) = [ α g α S O C ( k − 1 )   α g α U p 1 ( k − 1 )   α g α U p 1 ( k − 1 ) ] = [ U O C V ′ ( S O C ( k − 1 ) )   − 1   − 1 ] g ( X k − 1 ) = U O C V ( S O C ( k − 1 ) ) − U p 1 ( k − 1 ) − U p 2 ( k − 1 ) − I R 0 U k ≈ U O C V ( S O C ( k − 1 ) ) − U p 1 ( k − 1 ) − U p 2 ( k − 1 ) − I R 0 + [ U O C V ′ ( S O C ( k − 1 ) )   − 1   − 1 ] [ S O C ( k ) U p 1 ( k ) U p 2 ( k ) ] − [ U O C V ′ ( S O C ( k − 1 ) )   − 1   − 1 ] [ S O C ( k − 1 ) U p 1 ( k − 1 ) U p 2 ( k − 1 ) ] = H X k − I R 0 + D k − 1

$$\begin{align*} U_k&= g(X_{k})=U_{OCV}\left(SOC(k)\right)-U_{p1}(k)-U_{p2}(k)-I_kR_0+v_k\\ U_k&\approx g(X_{k-1}) + g'(X_{k-1})(X(k)-X(k-1))\\\\ H&=g'(X_{k-1})=\left[\frac{\alpha g}{\alpha SOC(k-1)}\ \frac{\alpha g}{\alpha U_{p1}(k-1)}\ \frac{\alpha g}{\alpha U_{p1}(k-1)}\right]\\ &=\left[U'_{OCV}(SOC(k-1))\ -1\ -1\right]\\\\ g(X_{k-1})&=U_{OCV}\left(SOC(k-1)\right)-U_{p1}(k-1)-U_{p2}(k-1)-IR_0\\\\\\\\ U_k&\approx U_{OCV}\left(SOC(k-1)\right)-U_{p1}(k-1)-U_{p2}(k-1)-IR_0\\ &+ \left[U'_{OCV}(SOC(k-1))\ -1\ -1\right] \left[ \begin{matrix}SOC(k)\\ U_{p1}(k)\\U_{p2}(k)\end{matrix} \right]\\ &- \left[U'_{OCV}(SOC(k-1))\ -1\ -1\right] \left[ \begin{matrix}SOC(k-1)\\U_{p1}(k-1)\\U_{p2}(k-1)\end{matrix} \right]\\ &=HX_{k}-IR_0+D_{k-1} \end{align*}$$ Uk​Uk​Hg(Xk−1​)Uk​​=g(Xk​)=UOCV​(SOC(k))−Up1​(k)−Up2​(k)−Ik​R0​+vk​≈g(Xk−1​)+g′(Xk−1​)(X(k)−X(k−1))=g′(Xk−1​)=[αSOC(k−1)αg​ αUp1​(k−1)αg​ αUp1​(k−1)αg​]=[UOCV′​(SOC(k−1)) −1 −1]=UOCV​(SOC(k−1))−Up1​(k−1)−Up2​(k−1)−IR0​≈UOCV​(SOC(k−1))−Up1​(k−1)−Up2​(k−1)−IR0​+[UOCV′​(SOC(k−1)) −1 −1] ​SOC(k)Up1​(k)Up2​(k)​ ​−[UOCV′​(SOC(k−1)) −1 −1] ​SOC(k−1)Up1​(k−1)Up2​(k−1)​ ​=HXk​−IR0​+Dk−1​​

上述过程可简化为以下流程，由于
$$U_{OCV}\left(SOC(k)\right)\approx U_{OCV}\left(SOC(k-1)\right)+U_{OCV}^{\prime }(SOC(k-1))(SOC(k)-SOC(k-1))$$
因此观测方程可写成
U k ≈ H X k − I k R 0 + D k − 1 + v k

$$\begin{align*}U_k&\approx HX_{k}-I_kR_0+D_{k-1}+v_k\\ \end{align*}$$ Uk​​≈HXk​−Ik​R0​+Dk−1​+vk​​
其中
$$H=\left(\begin{array}{c}{U}_{OCV}^{\prime }(SOC(k-1)),-1,-1\end{array}\right)$$

$$D_{k-1}=U_{OCV}\left(SOC(k-1)\right)-U_{OCV}^{\prime }(SOC(k-1))SOC(k-1)$$

根据卡尔曼滤波算法，算法步骤如下:

1.初始化：

初始状态为$X_0={\left(SOC(0),U_{p1}(0),U_{p2}(0)\right)}^T$，其协方差矩阵为$P_0$，$Q_k$设置为固定值$Q$，$R_k$设置为固定值$R$.

2.预测阶段：
$$X_k=A{X}_{k-1}+B{I}_k$$

$$P_k=A{P}_{k-1}{A}^T+Q_k$$

3.修正阶段：

卡尔曼增益为
$$K_k=\frac{P_k{H}^T}{H{P}_k{H}^T+R_k}$$
修正状态为
$$X_k^{\prime }=X_k+K_k(z_k-U_k)$$
修正状态的协方差矩阵为
$$P_k^{\prime }=\left(I-K_{k}H\right)P_k$$

2.4 基于无迹卡尔曼滤波的SOC估算

算法步骤：

1. 初始化

初始状态为$X_0={\left(SOC(0),U_{p1}(0),U_{p2}(0)\right)}^T$，其协方差矩阵为$P_0$，$Q_k$设置为固定值$Q$，$R_k$设置为固定值$R$.

2. 预测阶段

状态变量设置为$X_k={\left(SOC(k),U_{p1}(k),U_{p2}(k)\right)}^T$，电芯最大可用容量C，时间间隔为t，则状态方程为
$$X_k=A{X}_{k-1}+B{I}_k+{\omega }_k$$
其中，${\omega }_k\sim N(0,Q_k)$， A = ( 1 0 0 0 e − t C p 1 R p 1 0 0 0 e − t C p 1 R p 1 ) A=\left(

$$\begin{matrix}1&0&0\\0&e^{-\frac{t}{C_{p1}R_{p1}}}&0\\0&0& e^{-\frac{t}{C_{p1}R_{p1}}}\end{matrix}$$ \right) A= ​100​0e−Cp1​Rp1​t​0​00e−Cp1​Rp1​t​​ ​，$B=\left(\begin{array}{c}-\frac{t\times 100}{C}\\ R_{p1}(1-e^{-\frac{t}{C_{p1}{R}_{p1}}})\\ R_{p2}(1-e^{-\frac{t}{C_{p2}{R}_{p2}}})\end{array}\right)$，矩阵中参数根据$SOC(k)$进行匹配.

• 构造sigma点集，X有3个变量，n=3。

X k ( 0 ) = X k − 1 X k ( 1 ) = X k − 1 + ( ( n + λ ) P k − 1 ) 第 1 列 X k ( 2 ) = X k − 1 + ( ( n + λ ) P k − 1 ) 第 2 列 X k ( 3 ) = X k − 1 + ( ( n + λ ) P k − 1 ) 第 3 列

$$\begin{align} X^{(0)}_k&=X_{k-1}\\\\ X^{(1)}_k&=X_{k-1}+\left({\sqrt{(n+\lambda)P_{k-1}}}\right)_{第1列}\\ X^{(2)}_k&=X_{k-1}+\left({\sqrt{(n+\lambda)P_{k-1}}}\right)_{第2列}\\ X^{(3)}_k&=X_{k-1}+\left({\sqrt{(n+\lambda)P_{k-1}}}\right)_{第3列}\\\\ \end{align}$$ Xk(0)​Xk(1)​Xk(2)​Xk(3)​​=Xk−1​=Xk−1​+((n+λ)Pk−1​ ​)第1列​=Xk−1​+((n+λ)Pk−1​ ​)第2列​=Xk−1​+((n+λ)Pk−1​ ​)第3列​​​

X k ( 4 ) = X k − 1 − ( ( n + λ ) P k − 1 ) 第 1 列 X k ( 5 ) = X k − 1 − ( ( n + λ ) P k − 1 ) 第 2 列 X k ( 6 ) = X k − 1 − ( ( n + λ ) P k − 1 ) 第 3 列

$$\begin{align} X^{(4)}_k&=X_{k-1}-\left({\sqrt{(n+\lambda)P_{k-1}}}\right)_{第1列}\\ X^{(5)}_k&=X_{k-1}-\left({\sqrt{(n+\lambda)P_{k-1}}}\right)_{第2列}\\ X^{(6)}_k&=X_{k-1}-\left({\sqrt{(n+\lambda)P_{k-1}}}\right)_{第3列}\\\\ \end{align}$$ Xk(4)​Xk(5)​Xk(6)​​=Xk−1​−((n+λ)Pk−1​ ​)第1列​=Xk−1​−((n+λ)Pk−1​ ​)第2列​=Xk−1​−((n+λ)Pk−1​ ​)第3列​​​

• 各个sigma点代入状态方程计算

X k ( i ) − = A X k ( i ) + B I k X k − = ∑ i = 0 2 n w m i X k ( i ) − P k − = ∑ i = 0 2 n w c i [ X k ( i ) − − X k − ] [ X k ( i ) − − X k − ] T + Q

$$\begin{align} {X_k^{(i)}}^-&=AX_k^{(i)}+BI_k\\ X_{k}^-&=\sum_{i=0}^{2n}w_m^{i}{X_k^{(i)}}^-\\ P_k^-&=\sum_{i=0}^{2n}w_c^{i}[{X_k^{(i)}}^--X_{k}^-][{X_k^{(i)}}^--X_{k}^-]^{T}+Q\\ \end{align}$$ Xk(i)​−Xk−​Pk−​​=AXk(i)​+BIk​=i=0∑2n​wmi​Xk(i)​−=i=0∑2n​wci​[Xk(i)​−−Xk−​][Xk(i)​−−Xk−​]T+Q​​

其中，
{ w m ( 0 ) = λ n + λ w m ( i ) = λ 2 ( n + λ ) , i = 1 ∼ 2 n w c ( 0 ) = λ n + λ + ( 1 − a 2 + β ) w c ( i ) = λ 2 ( n + λ ) , i = 1 ∼ 2 n \left\{

$$\begin{array}{c} w_m^{(0)}=\frac{\lambda}{n+\lambda}\\ w_m^{(i)}=\frac{\lambda}{2(n+\lambda)},i=1 \sim 2n\\ w_c^{(0)}=\frac{\lambda}{n+\lambda}+\left(1-a^{2}+\beta\right) \\ w_c^{(i)}=\frac{\lambda}{2(n+\lambda)},i=1 \sim 2n\\ \end{array}$$ \right. ⎩ ⎨ ⎧​wm(0)​=n+λλ​wm(i)​=2(n+λ)λ​,i=1∼2nwc(0)​=n+λλ​+(1−a2+β)wc(i)​=2(n+λ)λ​,i=1∼2n​
无迹变换引入四个参数：$\alpha ,\beta ,\kappa ,\lambda$。其中$\lambda$满足：
$$\lambda ={\alpha }^2(n+\kappa )-n$$
应为是线性变换，上述的计算结果与下面先验计算一样
$$X_k^{-}=A{X}_{k-1}+B{I}_k$$

$$P_k^{-}=A{P}_{k-1}{A}^T+Q_k$$

3. 修正阶段

观测方程为
U k = U O C V ( S O C ( k ) ) − U p 1 ( k ) − U p 2 ( k ) − I k R 0 + v k

$$\begin{align*}U_k&= U_{OCV}\left(SOC(k)\right)-U_{p1}(k)-U_{p2}(k)-I_kR_0+v_k\\ \end{align*}$$ Uk​​=UOCV​(SOC(k))−Up1​(k)−Up2​(k)−Ik​R0​+vk​​
其中，$v_k\sim N(0,R_k)$.

• 构造sigma点集

X ( 0 ) = X k − X ( 1 ) = X k − + ( ( n + λ ) P k − ) 第 1 列 X ( 2 ) = X k − + ( ( n + λ ) P k − ) 第 2 列 X ( 3 ) = X k − + ( ( n + λ ) P k − ) 第 3 列

$$\begin{align} X^{(0)}&=X_k^-\\\\ X^{(1)}&=X_k^-+\left({\sqrt{(n+\lambda)P_{k}^-}}\right)_{第1列}\\ X^{(2)}&=X_k^-+\left({\sqrt{(n+\lambda)P_{k}^-}}\right)_{第2列}\\ X^{(3)}&=X_k^-+\left({\sqrt{(n+\lambda)P_{k}^-}}\right)_{第3列}\\\\ \end{align}$$ X(0)X(1)X(2)X(3)​=Xk−​=Xk−​+((n+λ)Pk−​ ​)第1列​=Xk−​+((n+λ)Pk−​ ​)第2列​=Xk−​+((n+λ)Pk−​ ​)第3列​​​

X ( 4 ) = X k − − ( ( n + λ ) P k − ) 第 1 列 X ( 5 ) = X k − − ( ( n + λ ) P k − ) 第 2 列 X ( 6 ) = X k − − ( ( n + λ ) P k − ) 第 3 列

$$\begin{align} X^{(4)}&=X_k^--\left({\sqrt{(n+\lambda)P_{k}^-}}\right)_{第1列}\\ X^{(5)}&=X_k^--\left({\sqrt{(n+\lambda)P_{k}^-}}\right)_{第2列}\\ X^{(6)}&=X_k^--\left({\sqrt{(n+\lambda)P_{k}^-}}\right)_{第3列}\\\\ \end{align}$$ X(4)X(5)X(6)​=Xk−​−((n+λ)Pk−​ ​)第1列​=Xk−​−((n+λ)Pk−​ ​)第2列​=Xk−​−((n+λ)Pk−​ ​)第3列​​​

• 计算各个sigma点的非线性变换，sigma点集代入观测方程，得到预测的观测量：

X ( i ) = ( S O C ( i ) , U p 1 ( i ) , U p 2 ( i ) ) T Z ( k ) ( i ) = U O C V ( S O C ( i ) ) − U p 1 ( i ) − U p 2 ( i ) − I k R 0

$$\begin{align} X^{(i)}&=\left(SOC^{(i)},U_{p1}^{(i)},U_{p2}^{(i)}\right)^T\\ Z(k)^{(i)}&=U_{OCV}(SOC^{(i)})-U_{p1}^{(i)}-U_{p2}^{(i)}-I_kR_0\\ \end{align}$$ X(i)Z(k)(i)​=(SOC(i),Up1(i)​,Up2(i)​)T=UOCV​(SOC(i))−Up1(i)​−Up2(i)​−Ik​R0​​​

• 通过加权求得观测量新的均值及协方差：

Z ˉ ( k ) = ∑ i = 0 2 n w m i Z ( k ) ( i ) P z k z k = ∑ i = 0 2 n w c i [ Z ( k ) ( i ) − Z ˉ ( k ) ] [ Z ( k ) i − Z ˉ ( k ) ] T + R P x k z k = ∑ i = 0 2 n w c i [ X ( i ) k − − X k − ] [ Z ( k ) i − Z ˉ ( k ) ] T

$$\begin{align} \bar{Z}(k)&=\sum_{i=0}^{2n}w_m^{i}Z(k)^{(i)}\\ P_{z_{k}z_{k}}&=\sum_{i=0}^{2n}w_c^{i}[Z(k)^{(i)}-\bar{Z}(k)][Z(k)^{i}-\bar{Z}(k)]^{T}+R\\ P_{x_{k}z_{k}}&=\sum_{i=0}^{2n}w_c^{i}[{X^{(i)}}_{k}^- -X_{k}^-][Z(k)^{i}-\bar{Z}(k)]^{T} \end{align}$$ Zˉ(k)Pzk​zk​​Pxk​zk​​​=i=0∑2n​wmi​Z(k)(i)=i=0∑2n​wci​[Z(k)(i)−Zˉ(k)][Z(k)i−Zˉ(k)]T+R=i=0∑2n​wci​[X(i)k−​−Xk−​][Z(k)i−Zˉ(k)]T​​

其中，
{ w m ( 0 ) = λ n + λ w m ( i ) = 1 2 ( n + λ ) , i = 1 ∼ 2 n w c ( 0 ) = λ n + λ + ( 1 − a 2 + β ) w c ( i ) = 1 2 ( n + λ ) , i = 1 ∼ 2 n \left\{

$$\begin{array}{c} w_m^{(0)}=\frac{\lambda}{n+\lambda}\\ w_m^{(i)}=\frac{1}{2(n+\lambda)},i=1 \sim 2n\\ w_c^{(0)}=\frac{\lambda}{n+\lambda}+\left(1-a^{2}+\beta\right) \\ w_c^{(i)}=\frac{1}{2(n+\lambda)},i=1 \sim 2n\\ \end{array}$$ \right. ⎩ ⎨ ⎧​wm(0)​=n+λλ​wm(i)​=2(n+λ)1​,i=1∼2nwc(0)​=n+λλ​+(1−a2+β)wc(i)​=2(n+λ)1​,i=1∼2n​

• 更新K值

$$K_k=P_{x_k{z}_k}{P}_{z_k{z}_k}^{-1}$$

• 系统状态更新和协方差更新

X ^ ( k ) = X k − + K k [ Z ( k ) − Z ˉ ( k ) ] P ( k ) = P k − − K k P z k z k K k T

$$\begin{align} \hat{X}(k)&=X_k^-+K_k[Z(k)-\bar{Z}(k)]\\ P(k)&=P_k^--K_k P_{z_{k}z_{k}} K^{T}_{k} \end{align}$$ X^(k)P(k)​=Xk−​+Kk​[Z(k)−Zˉ(k)]=Pk−​−Kk​Pzk​zk​​KkT​​​

2.3 基于自适应卡尔曼滤波

参考论文：《Adaptive Adjustment of Noise Covariance in Kalman Filter for Dynamic State Estimation》

一般情况下，kalman滤波中的Q、R都是根据经验、实验或数据手册得到的，但是有些参数是无法获得的，尤其是过程噪声，就需要通过不断试凑确定参数，显然是不靠谱的。

自适应卡尔曼滤波提供一种自适应调整Q、R的方法，不需要精确的初值，就可以获得较高的滤波精度。

• 只要Q、R的比值和Qtrue、Rtrue相等，就能获得最优的性能，并不需要保证Q、R和真实值接近。