基于模拟电路形式的卡尔曼滤波方法及模拟电路

技术领域

本发明涉及卡尔曼（Kalman)滤波算法，尤其涉及一种基于模拟电路形式的卡尔曼滤波方法及模拟电路。 

背景技术

从量测到的或者接收到的各种带有干扰的信号中取出有用信号的方法或技术称为滤波，或称为信号滤波。根据所需信号和量测值的统计特性，从量测值中得出某种统计意义上误差性能最小，亦即最优的信号估计，一般称为最优滤波或最优估计。 

1960年出现的卡尔曼（Kalman）滤波是一种线性最小方差滤波方法，是根据概率论、数理统计和线性代数理论发展起来的一门新兴技术，是现代控制工程中的一个主要方面。卡尔曼滤波理论的提出在工程上得到了广泛的应用，尤其在控制、制导、导航、通讯等现代工程方面。 

Kalman滤波考虑了信号与量测值的基本特性，而且采用状态空间的概念，因此，具有如下特点： 

（1）Kalman滤波以估计的均方误差最小为准则，且为无偏估计； 

（2）Kalman滤波算法是递推的，且在时域内设计滤波器，适用于多维随机过程的估计； 

（3）采用状态方程描述被估计量的动态变化规律，被估计量的动态统计信息由激励白噪声的统计信息和动力学方程确定。由于激励白噪声是平稳过程，动力学方程已知，所以被估计量既可以是平稳的也可以是非平稳的； 

（4）Kalman滤波的离散算法可以直接在计算机上实现。 

Kalman滤波算法目前都采用数字处理器（如单片机或DSP）来实现，因此传感器的模拟电压输出必须通过模数转换器（A/D）进行数字化。这种系统架构带来如下局限： 

（1）可靠性：系统包含模拟部分、数字部分和模数接口，结构较复杂，影响系统可靠性； 

（2）时间响应：响应速度受数字处理器处理速度和A/D转换频率限制，若 采用高速处理器，则将导致成本大幅上升； 

（3）成本：A/D转换器和数字处理器的使用显著提高成本。 

对于如惯性姿态测量等应用中的结构简单、维数低的Kalman滤波器，滤波参数可以设计为常值，因此可以考虑采用模拟电路来实现Kalman滤波，即采用运算放大器等模拟元器件实现加法、积分、反馈等滤波计算环节。 

发明内容

针对现有技术中存在的问题，针对较低维数的Kalman滤波的应用，设计模拟电路实现Kalman滤波算法。 

为了解决上述技术问题，本发明采用如下的技术方案： 

一种基于模拟电路形式的卡尔曼滤波方法，包括步骤： 

S1，基于被估计系统的各类输出信号及其与状态量的物理关系，构建系统的卡尔曼滤波模型，所述的卡尔曼滤波模型包括系统的状态量更新模型、状态模型和观测模型； 

S2，基于卡尔曼滤波模型获得系统的滤波参数，所述的滤波参数包括卡尔曼滤波的稳态增益； 

S3，基于卡尔曼滤波模型所获的滤波参数构建比例积分运算模块或比例运算模块、以及积分运算模块和差分运算模块； 

S4，采用模拟元器件分别搭建所述的比例积分运算模块或比例运算模块，以及差分运算模块和积分运算模块的模拟电路，获得卡尔曼滤波算法的模拟电路实现形式；所述的模拟元器件主要为运算放大器； 

S5，采用卡尔曼滤波模拟电路对系统的输出信号进行卡尔曼滤波。 

上述步骤S2进一步包括子步骤： 

所述的步骤S2进一步包括子步骤： 

2-1基于系统的各类输出信号，构建系统的状态模型和观测模型； 

2-2基于系统的状态模型和观测模型构建连续时间的卡尔曼滤波模型，并获得卡尔曼滤波模型的稳态解； 

2-3根据卡尔曼滤波模型的稳态解获得系统的滤波参数。 

上述步骤S3具体为： 

基于系统的状态量更新模型构建第一差分运算模块，基于系统的状态模型构 建积分运算模块，基于待滤波系统的观测模型构建第二差分运算模块，基于卡尔曼滤波的稳态增益构建比例积分运算模块或比例运算模块。 

上述差分运算模块具体为电压加减运算模块。 

一种卡尔曼滤波模拟电路，包括： 

比例积分运算模块或比较运算模块中的一种、以及第一差分运算模块、积分运算模块和第二差分运算模块。 

系统观测量信号与上一时刻的状态量估计值经第二差分运算模块处理后得到新息，新息经比例积分运算模块或比较运算模块后获得滤波增益，所得滤波增益与输入信号依次经第一差分运算模块进行状态量更新、经积分运算模块获得状态量最新估计值，然后，进入下一周期的状态量估计。 

上述比例积分运算模块、比例运算模块、差分运算模块、积分运算模块的核心器件均为运算放大器.。 

上述差分运算模块具体为电压加减运算模块。 

Kalman滤波能够从一系列包含噪声的观测量中估计动态系统的状态。传统的数字形式Kalman滤波的实现方式比较复杂，且难以集成，并且其中的A/D（模/数）转换可能引起信号的时间延迟；而模拟电路实现的Kalman滤波器系统架构简单，可靠性高，时间响应快且由于省去了A/D转换环节和数字处理器，大大降低了成本。另外，模拟电路实现的Kalman滤波器易于与传感器集成。 

采用基本模拟电子器件构建的模拟电路来实现Kalman滤波，可实现模拟信号的最优估计，可避免模拟信号和数字信号之间的转换，具有如下优势： 

（1）大大简化了系统架构，可提高可靠性和抗干扰能力； 

（2）模拟电路几乎没有时间滞后，时间响应快； 

（3）省去了A/D转换器和数字处理器，仅需要运算放大器和电阻电容等模拟器件，成本几乎可以忽略。 

附图说明

图1是本具体实施中稳态Kalman滤波器的系统框图，其中，图（b）为图（a）的等效系统流程图； 

图2是本具体实施的Kalman滤波模拟电路； 

图3是本发明Kalman滤波模拟电路的结构示意图； 

图4是本具体实施和传统的数字电路实现的Kalman滤波器的滤波结果对比，图（a）为两者角度估计结果对比，图（b）为两者角度估计差异，图（c）为陀螺角速率信号。 

具体实施方式

本发明基于连续时间的Kalman滤波方程，并根据黎卡提(Riccati)方程的求解方法，得到Kalman滤波方程的稳态解，滤波参数（如增益矩阵）在进入稳态后变为常值，从而使得各个运算环节（如加法、积分、比例运算等）可以用运算放大器等模拟元器件来实现，实验表明，模拟电路实现的Kalman滤波器得到的估计结果与数字方式实现的Kalman滤波器结果一致，说明模拟电路实现Kalman滤波器是可行的。 

模拟器件与模拟电路本身也存在一些误差，比如运放漏电流、偏置电压等，但是可通过对模拟器件的合理选型以及电路的合理设计，来减小模拟电路本身引起的误差。另外，有些误差甚至可以与传感器误差合并考虑，作为Kalman滤波算法中的待估参数进行在线估计与补偿。 

Kalman滤波方程的求解过程如下： 

假设系统的状态方程： 

$\stackrel{·}{x}\left(t\right)=F\left(t\right)x\left(t\right)+B\left(t\right)u\left(t\right)+G\left(t\right)w\left(t\right)---\left(1\right)$

其中，x(t)为系统的n维状态向量，F(t)为n×n维系统矩阵，u(t)为r维系统控制向量，B(t)为n×r维确定性输入矩阵，w(t)为s维系统噪声向量，G(t)是n×s维噪声输入矩阵。 

观测方程为： 

z(t)=H(t)x(t)+v(t)     （2） 

式中，z(t)是m维观测向量，H(t)是m×n维观测矩阵，v(t)是m维量测噪声向量。 

系统噪声与观测噪声相互独立，且系统噪声的协方差和观测噪声的协方差分别表示为： 

$\left(\begin{array}{c}E\left[w\left(t\right)w^T\left(t\right)\right]=Q\left(t\right)\delta (t-\tau )\\ E\left[v\left(t\right)v^T\left(t\right)\right]=R\left(t\right)\delta (t-\tau )\end{array}\right)---\left(3\right)$

其中，Q(t)表示系统噪声的方差矩阵，R(t)表示观测噪声的方差矩阵，δ(t-τ)表示狄拉克函数（Dirac Delta function）。 

根据建立的状态方程与观测方程，可以得到连续时间Kalman滤波方程： 

$\left(\begin{array}{c}\stackrel{\stackrel{·}{^}}{x}\left(t\right)=F\left(t\right)\hat{x}\left(t\right)+B\left(t\right)u\left(t\right)+K\left(t\right)[z\left(t\right)-H\left(t\right)\hat{x}\left(t\right)]\\ K\left(t\right)=P\left(t\right)H^T\left(t\right){R_c}^{-1}\left(t\right)\\ \stackrel{·}{P}\left(t\right)=F\left(t\right)P\left(t\right)+P\left(t\right)F^T\left(t\right)+G\left(t\right)Q\left(t\right)G^T\left(t\right)-K\left(t\right)R_c\left(t\right)K^T\left(t\right)\end{array}\right)---\left(4\right)$

其中，是状态量x的估计，P(t)是估计误差方差矩阵，K(t)是Kalman滤波增益。 

下面结合附图及实施例，对本发明作进一步详细的描述。 

以倾角检测系统为例，系统包含一个单轴陀螺（型号ADXRS300）和一个倾角计，倾角计主要由ADXL202型号的加速度计传感器实现，陀螺测量表示动态变化的角速率，对角速率进行积分可以得到包含角度漂移的倾角。倾角计直接测量静态角度，这两种传感器具有互补特性，因此将两者的输出信号通过Kalman滤波器进行组合，可以得到角度的最优估计。 

结合本实例，单轴陀螺输出信号的连续时间模型如下： 

${\Omega }_o\left(t\right)={\Omega }_t\left(t\right)-w_1\left(t\right)-w_g\left(t\right)---\left(3\right)$

式（5）中，Ω_o(t)表示陀螺的输出信号，其下标“o”表示输出output；Ω_t(t)表示角速率真值，其下标“t”表示真值true；w₁(t)表示白噪声，w_g(t)是陀螺的零偏，括号中的t表示连续时间。 

w_g(t)通常可模型为变化缓慢的随即游走： 

${\stackrel{·}{w}}_g\left(t\right)=w_2\left(t\right)---\left(6\right)$

式（6）中，w₂(t)为零均值的高斯白噪声。 

倾角计输出信号的模型如下： 

${\psi }_o\left(t\right)={\psi }_t\left(t\right)+v\left(t\right)---\left(7\right)$

其中，ψ_o(t)表示倾角计的输出信号，ψ_t(t)表示真实角度，v(t)表示倾角计的测量噪声。 

将真实角度ψ_t和陀螺的零偏w_g作为系统的状态量x，陀螺输出Ω_o作为状态方程的控制量u，倾角计的输出作为系统的量测值z： 

$x=\left(\begin{array}{c}{x}_1\\ x_2\end{array}\right)=\left(\begin{array}{c}{\psi }_t\\ w_g\end{array}\right),z={\psi }_o,u={\Omega }_o---\left(8\right)$

根据式（8）得到： 

${\Omega }_t\left(t\right)={\Omega }_o\left(t\right)+w_1\left(t\right)+w_g\left(t\right)---\left(9\right)$

$\left(\begin{array}{c}{\stackrel{·}{x}}_1\left(t\right)={\Omega }_t\left(t\right)=x_2\left(t\right)+u\left(t\right)+w_1\left(t\right)\\ {\stackrel{·}{x}}_2\left(t\right)=w_2\left(t\right)\end{array}\right)---\left(10\right)$

$z\left(t\right)={\psi }_o\left(t\right)={\psi }_t\left(t\right)+v\left(t\right)=x_1\left(t\right)+v\left(t\right)---\left(11\right)$

根据连续时间Kalman滤波方程，得到本实施例的Kalman滤波模型： 

状态方程为： 

$\left(\begin{array}{c}{\stackrel{·}{x}}_1\\ {\stackrel{·}{x}}_2\end{array}\right)=\left(\begin{array}{cc}0& 1\\ 0& 0\end{array}\right)·\left(\begin{array}{c}{x}_1\\ x_2\end{array}\right)+\left(\begin{array}{c}1\\ 0\end{array}\right)·u+\left(\begin{array}{c}{w}_1\\ w_2\end{array}\right)---\left(12\right)$

量测方程为： 

$z=\left(\begin{array}{cc}1& 0\end{array}\right)·\left(\begin{array}{c}{x}_1\\ x_2\end{array}\right)+v---\left(13\right)$

即本实施例的系统矩阵$F=\left(\begin{array}{cc}0& 1\\ 0& 0\end{array}\right),$确定性输入矩阵$B=\left(\begin{array}{c}1\\ 0\end{array}\right),$观测矩阵 $H=\left(\begin{array}{cc}1& 0\end{array}\right).$

根据式(1)假设的系统方程、式(2)的观测方程和式(4)的连续时间卡尔曼滤波方程得到卡尔曼滤波增益矩阵K与估计误差方差矩阵P： 

$K=\left(\begin{array}{c}{k}_1\\ k_2\end{array}\right)=P\left(t\right)·\left(\begin{array}{c}1\\ 0\end{array}\right)·R^{-1}---\left(14\right)$

$\stackrel{.}{P}\left(t\right)=\left(\begin{array}{cc}0& 1\\ 0& 0\end{array}\right)·P\left(t\right)+P\left(t\right)·{\left(\begin{array}{cc}0& 1\\ 0& 0\end{array}\right)}^T+Q-\left(\begin{array}{c}{k}_1\\ k_2\end{array}\right)·R·\left(\begin{array}{cc}{k}_1& k_2\end{array}\right)---\left(15\right)$

式（14）～（15）中，系统噪声方差矩阵R和观测噪声方差矩阵Q的初始值根据选用的传感器的精度确定。 

之和，由Riccati方程的求解方法，上式（15）的微分方程的解可表示为： 

$P\left(t\right)=Y\left(t\right){\Lambda }^{-1}\left(t\right)---\left(16\right)$

P(t)的初始值P(t₀)为： 

$P\left(t\right){|}_{t=t_0}=\mathrm{var}\left(x_0\right)=P_0---\left(17\right)$

其中， 

$\left(\begin{array}{c}Y\left(t\right)\\ \Lambda \left(t\right)\end{array}\right)=\Theta (t,t_0)\left(\begin{array}{c}Y\left(t_0\right)\\ \Lambda \left(t_0\right)\end{array}\right)=\left(\begin{array}{cc}{\Theta }_{11}& {\Theta }_{12}\\ {\Theta }_{21}& {\Theta }_{22}\end{array}\right)\left(\begin{array}{c}{p}_0\\ I\end{array}\right)---\left(18\right)$

t₀表示起始时间；P₀为估计误差方差矩阵P的初始值；var(x₀)表示状态初值方差；I为单位矩阵；Λ为对角矩阵；Θ(t,t₀)是微分方程的一步状态转移矩阵。 

因此Kalman滤波的稳态解为： 

$\left(\begin{array}{c}P\left(t\right)=Y\left(t\right){\Lambda }^{-1}\left(t\right)=({\Theta }_{11}{P}_0+{\Theta }_{12}){({\Theta }_{21}{P}_0+{\Theta }_{22})}^{-1}\\ K\left(t\right)=P\left(t\right)H^T\left(t\right){R_c}^{-1}\left(t\right)\end{array}---\left(19\right)\right)$

根据上式（19）解得的Kalman滤波的稳态增益K。 

根据上述分析，基于Kalman滤波中的状态量更新过程（见式（4））构建第一加减运算模块，基于系统的状态方程（见式（1））构建积分运算模块，基于观测方程构建第二加减运算模块，基于Kalman滤波的稳态增益K构建比例积分运算模块或比例运算模块，从而获得本实施例倾角检测系统的稳态Kalman滤波器的系统框图，见图1（a），图中，x₁和x₂为状态量，k₁和k₂表示Kalman增益。等效简化系统框图并转换成其在拉普拉斯变换域的形式，见图1（b）。 

稳态Kalman滤波器主要由比例积分运算模块和比例运算模块中的一种、加减运算模块、以及积分运算模块组成，上述基本模块非常适合利用基本的模拟器件来实现，例如采用运算放大器、电阻、电容等基本模拟器件来实现。 

本具体实施根据Kalman滤波的稳态增益参数，主要采用LM224运算放大器、电阻和电容分别实现加减运算模块、积分运算模块、比例积分运算模块、比例运算模块，见图2。 

图2中，运算放大器U1与电阻R₁、R₂、R₃、R₄组成加减运算模块A，R₁=R₂，R₃=R₄，实现陀螺信号与估计值的相减运算： 

$V_1=\frac{R_4}{R_4}(V_3-{\Omega }_g)---\left(20\right)$

运算放大器U2与电阻R₇、R₈、C₉和电容C₂组成比例积分运算模块D： 

$V_3=-(\frac{1}{R_8{C}_2}\int V_2\mathrm{dt}+\frac{R_7}{R_8}{V}_2)---\left(21\right)$

运算放大器U3与电阻R₁₀、R₁₁、R₁₂、R₁₃组成加减运算模块C： 

$V_2=\frac{R_{11}}{R_{10}}({\Psi }_o-\hat{\Psi })---\left(22\right)$

运算放大器U4与电阻R₅、R₆、电容C₁组成积分运算模块B： 

$\hat{\Psi }=-\frac{1}{R_5{C}_1}\int V_1\mathrm{dt}---\left(23\right)$

图2中所示的电路图与图1（b）中的简化系统框图是等效的，运放型号与电阻、电容的值根据所获得的卡尔曼滤波参数值进行选择，通过合理的器件选择与电路设计，可以最大限度减小模拟电路的内在缺陷（如运放漏电流、偏置电压等）的影响。 

将设计的模拟电路实现的Kalman滤波器放置于一个分度头（可以产生倾斜角的仪器）上进行测试，可以实时得到模拟实现的卡尔曼滤波的估计结果。另一方面，测试中的所有模拟信号（陀螺、倾角计、滤波器输出）以500Hz采样率，由一个16位的A/D采集卡进行采集，将记录的数字化的信号通过数字Kalman滤波器进行估计。将本发明模拟电路实现的Kalman滤波器估计结果与数字实现的Kalman滤波器估计结果进行对比，对比结果见图4。图4（a）为测试中的倾角计测量信号、陀螺信号的积分结果，模拟电路方式实现的Kalman滤波估计结果，数字方式实现的Kalman滤波估计结果；图4（b）为本发明模拟电路方式实现的Kalman滤波器与数字Kalman滤波器的估计结果之差曲线；图4（c）为陀螺采集的角速率信号。 

图4（a）表明在测量范围+/-10°的条件下，两种方式的Kalman滤波器估计结果一致。如图4（b）表明两种实现方式的估计差异与测量的倾角范围和角速率大小有关。综合图4中的三个副图，两种实现方式的估计差异可以分成动态差异（图中标记为“1”）与静态差异（图中标记为“2,3,4,5”）两种，其中动态差异主要与角速率相关，且主要由陀螺传感器的比例因子误差和信号经过低通滤波器后的时间延迟产生；静态差异主要与倾角的大小有关，倾角计在计算角度(θ)时直接将其加速度计的输出(a)近似作为角度结果，这是产生静态差异的主要原因： 

θ=arcsin(a/g)≈a/g 

其中，g表示重力加速度。近似误差如表1所示，图4中的结果与近似误差 的理论值一致。当测量范围在+/-20°时，模拟电路实现的Kalman滤波器的估计结果与数字方式的结果的差异仅为0.4°。需要指出的是，该近似误差是本实例所特有的，并非本发明的模拟Kalman滤波器本身的特性。 

表1近似误差 

本实施例的应用范围包含但不仅限于对角度信号的估计，还可以应用于要求高可靠性、高动态、低成本的动态系统中。 

如图3中所示，本发明的Kalman滤波模拟电路包含如下四个核心模块：第一加减运算模块A、积分模块B实现、第二加减运算模块C及基于Kalman滤波的稳态增益构建的比例积分运算模块或比例运算模块。对于包含传感器误差估计的系统，则基于Kalman滤波的稳态增益构建比例积分模块D来实现Kalman滤波的稳态增益；对于无传感器误差估计的系统，则基于Kalman滤波的稳态增益直接构建比例运算模块来实现Kalman滤波的稳态增益。参见图3，观测量信号与上次的状态估计值经第二加减运算模块C得到新息、新息经比例积分运算模块或比例运算模块，并与输入信号一起经过第一加减运算模块A、以及积分模块B，最后得到状态量的最新的估计值。