并行卡尔曼滤波系统和方法

技术领域

本发明涉及通讯技术领域，具体涉及一种通讯技术领域的数字信号并行卡 尔曼滤波方法。

背景技术

滤波理论是在对系统可观察信号进行测量的基础上，根据一定的滤波准则， 对系统的状态进行估计的理论和方法。根据贝叶斯理论，最优估计只是一个理 想化的解，一般情况下，无法得到它的解析形式。20世纪60年代，由R.E.Kalman 提出了卡尔曼滤波方法，在时域内以递推方式进行估计，其滤波标准是均方误 差最小，当系统为线性高斯分布时，它可以得到递归贝叶斯估计的最小均方误 差解，此方法克服了经典维纳滤波算法的局限，是最优化的自回归数据处理算 法。也是目前最经典、应用最广泛的线性滤波方法。此算法选用状态空间方法 来设计滤波器，能对多维系统和非平稳系统的随机过程进行估计。卡尔曼滤波 器因其自身的优点：递归运算、自适应性、前瞻性等，能够对随机干扰下的线 性动态系统进行最优估计，因而有着广泛的应用。卡尔曼滤波已经被应用于航 空航天、通信工程、人工智能、图像处理、轨道交通、石油勘探等许多领域。 但传统的卡尔曼滤波方法，运行效率低，占用了较多的计算资源，无法同时实 现多个状态的卡尔曼滤波。

发明内容

本发明要解决的技术问题是提供一种并行卡尔曼滤波方法，其相对于传统 的卡尔曼滤波方法，能够有效的提升卡尔曼滤波的运行效率，可以同时实现多 个状态的卡尔曼滤波，也兼顾了计算效率与计算资源之间的平衡问题。

为了解决上述技术问题，本发明提供了一种并行卡尔曼滤波方法，包括以 下步骤，将卡尔曼的运算流程拆分为多个步骤，在使每个步骤之间进行并行运 算的同时，也使每个步骤用多个并行的子模块进行同时运算。

本发明一个较佳实施例中，进一步包括卡尔曼运算流程至少拆分成以下步 骤：S1，生成传输矩阵A；S2，生成观察矩阵H；S3，计算系统的状态转移； S4，计算观察测量值；S5，生成状态预测；S6，生成预测协方差P_pre(k)；S7， 计算卡尔曼增益；S8，计算状态最优估算值；S9，更新状态协方差；S10，产 生测量过程中遇到的噪声V和状态转移过程中产生的噪声W。

本发明一个较佳实施例中，进一步包括以下步骤，

(A)计算传输矩阵A：取A为n*n维矩阵

(B)计算观察矩阵H：取H为n*n维矩阵

(C)计算系统的状态值：由状态转移方程X(k)＝A*X(k-1)+L*W(k-1)计算 系统的状态转移；

(D)计算观察测量值：采用步骤(B)中计算出的矩阵

(E)生成状态预测：采用步骤(A)计算传输矩阵

(F)生成预测协方差P_pre(k)：取P_pre(k)为n*n维矩阵

(G)计算卡尔曼增益：取卡尔曼增益Kg为n维列向量

(H)计算状态最优估算值：取状态最优估算值

(I)更新状态协方差：其计算方程为P(k)＝(I-Kg(k)*H)*P_pre(k)；

(J)(K)分别为是为了产生测量过程中遇到的噪声和状态转移过程中产 生的噪声

一种并行卡尔曼滤波系统，包括传输矩阵生成模块、观察矩阵生成模块、 状态转移计算模块、观察测量计算模块、状态预测生成模块、预测协方差生成 模块、卡尔曼增益计算模块、状态最优估算值计算模块、状态协方差更新模块、 测量过程中的噪声生成模块、状态转移过程中产生的噪声生成模块、过程噪声 协方差生成模块和测量噪声协方差生成模块；状态转移计算模块、观察测量计 算模块、状态预测生成模块、预测协方差生成模块和卡尔曼增益计算模块均与 传输矩阵生成模块和观察矩阵生成模块连接；状态最优估算值计算模块和状态 协方差更新模块均与观察矩阵生成模块连接；预测协方差生成模块、卡尔曼增 益计算模块、测量过程中的噪声生成模块和状态转移过程中产生的噪声生成模 块均与过程噪声协方差生成模块和测量噪声协方差生成模块连接；测量过程中 的噪声生成模块和状态转移过程中产生的噪声生成模块分别与观察测量计算模 块和状态转移计算模块连接。

本发明一个较佳实施例中，进一步包括(1)传输矩阵生成模块能够完成步 骤(A)中功能；

(2)观察矩阵生成模块能够完成步骤(B)中功能；

(3)状态转移计算模块包括并行的多个独立的子模块，每一个计算状态转 移的子模块均能够完成步骤(C)中的完整功能X(k)＝A*X(k-1)+L*W(k-1)；

(4)观察测量计算模块包括并行的多个独立的子模块，每一个观察测量计 算子模块均能够完成步骤(D)中的完整功能Z(k)＝H*X(k)+V(k)；

(5)状态预测生成模块包括并行的多个独立的子模块，每一个状态预测生 成子模块都能够完成步骤(E)中的完整功能X_pre(k)＝A*Xkf(k-1)；

(6)预测协方差生成模块包括并行的多个独立的子模块，每一个预测协方 差生成子模块都能够完成步骤(F)中的完整功能P_pre(k)＝A*P(k-1)*A'+Q；

(7)卡尔曼增益计算模块包括并行的多个独立的子模块，每一个卡尔曼增 益计算子模块都能够完成步骤(G)中的完整功能Kg(k)＝P_pre(k)*H'*inv(H*P_pre(k)*H'+R)；

(8)状态最优估算值计算模块包括并行的多个独立的子模块，每一个状态 最优估算值计算子模块都能够完成步骤(H)中的完整功能 Xkf(k)＝X_pre(k)+Kg(k)*(Z(k)-H*X_pre(k)；

(9)状态协方差更新模块包括并行的多个独立的子模块，每一个状态协方 差更新子模块都能够完成步骤(I)中的完整功能P(k)＝(I-Kg(k)*H)*P_pre(k)；

(10)测量过程中的噪声生成模块包括并行的多个独立的子模块，每一个 测量过程中噪声生成子模块都能够完成步骤(J)中的完整功能；

(11)状态转移过程中产生的噪声生成模块包括并行的多个独立的子模块， 每一个传输噪声生成子模块都能够完成步骤(K)中的完整功能。

本发明一个较佳实施例中，进一步包括各模块或子模块为ASIC功能模块、 一个或多个CPU/GPU核，或者一个或多个FPGA单元。

一种并行卡尔曼滤波子模块数据传递及运算方法，包括以下步骤：

1)AM计算生成的传输矩阵

2)AM计算生成的传输矩阵

3)AM计算生成的传输矩阵

4)HM计算生成的观察矩阵

5)HM计算生成的观察矩阵

6)SESM1计算生成的X(1)＝系统初始状态通过路径6_1传送给SESM2， SESM2计算生成的

7)WGSM1计算生成的

8)SESM1计算生

9)VGSM1计算生成的

10)SUESM1计算生成的

11)CUESM1计算生成的

12)QGM计算生成的过程噪声协方差矩阵

13)CPESM1计算生成的P_pre(1)＝系统预测协方差初始值通过路径13_1 传送给KGESM1，CPESM2计算生成的

14)RGM计算生成的测量协方差R通过路径14_1，14_2，…，14_n-1， 14_n分别传送给KGESM1，KGESM2，…，KGESMn-1，KGESMn；

15)SPESM1计算生成的

16)KGESM1计算生成的Kg

17)OESM1计算生成的

18)KGESM1计算生成的Kg

19)CPESM1计算生成的

20)RGM计算生成的测量噪声协方差R通过路径20_1，20_2，…，20_n-1， 20_n分别传送给VGSM1，VGSM2，…，VGSMn-1，VGSMn；

21)QGM计算生成的过程噪声协方差矩阵

本发明一个较佳实施例中，进一步包括载波追踪的状态点的个数为20000 个，选用多普勒频偏DetaTheta，频偏的变化率Omega0，频偏变化率的变化率 Omega1作为系统的状态量，系统为三阶系统，detav＝1e-3，detaw＝1e-3，Ts＝1e-3，

本发明一个较佳实施例中，进一步包括n为500，则，SESMk完成 X(k)＝A*X(k-1)+L*W(k-1)；

(A)OESMk完成Z(k)＝H*X(k)+V(k)；

(C)CPESMk完成P_pre(k)＝A*P(k-1)*A'+Q；

(D)KGESMk完成Kg(k)＝P_pre(k)*H'*inv(H*P_pre(k)*H'+R)；

(E)SUESMk完成Xkf(k)＝X_pre(k)+Kg(k)*(Z(k)-H*X_pre(k))；

(F)CUESMk完成P(k)＝(I-Kg(k)*H)*P_pre(k)。

本发明一个较佳实施例中，进一步包括以下步骤，

1)完成6轮运算，产生3000个系统状态值；

2)设定系统状态量的初始值Omega1即X(3，3000)＝80*10^(3)，状态最优估 算值的初始值Xkf(3，300)＝80*10^(3)，其他数值保持不变，完成1轮运算，产 生500个系统状态值；

3)设定系统状态量的初始值Omega1即X(3，3500)＝0，状态最优估算值的 初始值Xkf(3，3500)＝0，其他数值保持不变，完成11轮运算，产生5500个系 统状态值；

4)设定系统状态量的初始值Omega1即X(3，9000)＝-80*10^(3)，状态最优 估算值的初始值Xkf(3，3500)＝-80*10^(3)，其他数值保持不变，完成1轮运算， 产生500个系统状态值；

5)设定系统状态量的初始值Omega1即X(3，9500)＝0，状态最优估算值的 初始值Xkf(3，3500)＝0，其他数值保持不变，完成12轮运算，产生6000个系 统状态值；

6)设定系统状态量的初始值Omega1即X(3，15500)＝80*10^(3)，状态最优 估算值的初始值Xkf(3，3500)＝80*10^(3)，其他数值保持不变，完成1轮运算， 产生500个系统状态值；

7)设定系统状态量的初始值Omega1即X(3，16000)＝0，状态最优估算值的 初始值Xkf(3，3500)＝0，其他数值保持不变，完成8轮运算，产生4000个系统 状态值。

本发明一个较佳实施例中，进一步包括。

本发明的有益效果：

本发明的并行卡尔曼滤波方法，其相对于传统的卡尔曼滤波方法，能够有 效的提升卡尔曼滤波的运行效率，可以同时实现多个状态的卡尔曼滤波，也兼 顾了计算效率与计算资源之间的平衡问题。

附图说明

图1为本发明优选实施例中并行卡尔曼滤波系统的结构示意图；

图2为本发明优选实施例中并行卡尔曼滤波系统的局部放大示意图一；

图3为本发明优选实施例中并行卡尔曼滤波系统的局部放大示意图二；

图4为本发明优选实施例中并行卡尔曼滤波系统的运算原理图；

图5为并行卡尔曼滤波子模块间数据传递及运算关系示意图；

图6为500个子模块的并行卡尔曼滤波子模块间数据传递及运算关系示意 图；

图7为实例计算生成的20000个系统状态中的多普勒频偏DetaTheta；

图8为实例计算生成的20000个系统状态中的多普勒频偏的变化率 Omega0；

图9为实例计算生成的20000个系统状态中的多普勒频偏变化率的变化率Omega1；

图10为实例计算生成的20000个系统状态的测量错误和卡尔曼估计误差。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人 员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。

实施例

本发明实施例公开一种并行卡尔曼滤波方法，参照图1～4所示，包括以下 步骤：将卡尔曼的运算流程拆分为多个步骤，在使每个步骤之间进行并行运算 的同时，也使每个步骤用多个并行的子模块进行同时运算。其相对于传统的卡 尔曼滤波方法，能够有效的提升卡尔曼滤波的运行效率，可以同时实现多个状 态的卡尔曼滤波，也兼顾了计算效率与计算资源之间的平衡问题。

在拆分卡尔曼运算流程时，至少包括以下步骤：

S1，生成传输矩阵A；

S2，生成观察矩阵H；

S3，计算系统的状态转移；

S4，计算观察测量值；

S5，生成状态预测；

S6，生成预测协方差P_pre(k)；

S7，计算卡尔曼增益；

S8，计算状态最优估算值；

S9，更新状态协方差；

S10，产生测量过程中遇到的噪声V和状态转移过程中产生的噪声W。

具体而言，参照图4所示，包括以下步骤：

(A)传输矩阵A生成部分用来计算出传输矩阵A(这里我们取A为n*n 维矩阵

(B)观察矩阵H生成部分用来计算出观察矩阵H(这里我们取H为n*n 维矩阵

(C)状态转移计算部分用来计算系统的状态值，系统的状态转移的计算 过程是根据状态转移方程X(k)＝A*X(k-1)+L*W(k-1)来进行的，其中X(这里我们 取X为n维列向量

(D)观察测量值的计算用(B)中计算出的矩阵

(E)状态预测生成部分用(A)计算出传输矩阵

(F)预测协方差生成部分用来计算预测状态的协方差P_pre(k)(这里我们 取P_pre(k)为n*n维矩阵

(G)卡尔曼增益计算部分是用来计算卡尔曼增益Kg(KalmanGain)(这 里我们取Kg为n维列向量

其中 Kgd11＝((h11*P

(H)状态最优估算值计算部分用来计算产生状态最优估算值

表示成矩阵形式为，

(I)状态协方差更新部分是通过(G)生成的卡尔曼增益

其中 P11(k)＝((1-(Kg11(k)*h11+…+Kg1n(k)*hn1))*P_pre11(k)+…+ (-(Kg11(k)*h1n+…+Kg1n(k)*hnn))*P

。

(J)(K)分别为是为了产生测量过程中遇到的噪声和状态转移过程中产 生的噪声

本发明实施例公开一种并行卡尔曼滤波系统，参照图1～5所示，包括传输 矩阵生成模块、观察矩阵生成模块、状态转移计算模块、观察测量计算模块、 状态预测生成模块、预测协方差生成模块、卡尔曼增益计算模块、状态最优估 算值计算模块、状态协方差更新模块、测量过程中的噪声生成模块、状态转移 过程中产生的噪声生成模块、过程噪声协方差生成模块和测量噪声协方差生成 模块。状态转移计算模块、观察测量计算模块、状态预测生成模块、预测协方 差生成模块和卡尔曼增益计算模块均与传输矩阵生成模块和观察矩阵生成模块 连接。状态最优估算值计算模块和状态协方差更新模块均与观察矩阵生成模块 连接。预测协方差生成模块、卡尔曼增益计算模块、测量过程中的噪声生成模 块和状态转移过程中产生的噪声生成模块均与过程噪声协方差生成模块和测量 噪声协方差生成模块连接。测量过程中的噪声生成模块和状态转移过程中产生 的噪声生成模块分别与观察测量计算模块和状态转移计算模块连接。

(1)传输矩阵生成模块(AModule)能够完成步骤(A)中功能。

(2)观察矩阵生成模块(HModule)能够完成步骤(B)中功能。

(3)将状态转移计算模块(StateEquationModule)用并行的多个独立的子 模块(SESubModule)来实现：状态转移计算子模块1(SESubModule1)，计算 X(1):X(1)＝系统初始状态。状态转移计算子模块2(SESubModule2)，计算 X(2):X(2)＝A*X(1)+L*W(1)。状态转移计算子模块N-1(SESubModuleN-1)，计 算X(N-1):X(N-1)＝A*X(N-2)+L*W(N-2)。状态转移计算子模块N (SESubModuleN)，计算X(N):X(N)＝A*X(N-1)+L*W(N-1)。每一个状态转移计 算子模块都能够完成步骤(C)中所述的完整功能X(k)＝A*X(k-1)+L*W(k-1)， 这样N个状态转移计算子模块就可以同时计算出N个不同的系统状态X(N)。 当N＝K时，可以用N个子模块中的K个子模块直接 一次将系统的K个状态X(1)，X(2)…X(K-1)X(K)全部计算出来。

(4)将观察测量计算模块(ObserveEquationModule)用并行的多个独立的 子模块(OESubModule)来实现：观察测量计算子模块1(OESubModule1)， 计算Z(1):Z(1)＝H*X(1)+V(1)。观察测量计算子模块2(OESubModule2)，计算 Z(2):Z(2)＝H*X(2)+V(2)。观察测量计算子模块N-1(OESubModuleN-1)，计算 Z(N-1):Z(N-1)＝H*X(N-1)+V(N-1)。观察测量计算子模块N(SESubModuleN)， 计算Z(N):Z(N)＝H*X(N)+V(N)。每一个观察测量计算子模块都能够完成步骤 (D)中所述的完整功能Z(k)＝H*X(k)+V(k)，这样N个观察测量计算子模块就可以同时计算出N个不同的观察测量值Z(N)。当N＝K时，可以用N个子模块中的K个子模块直接一次将系统的K个观察测量 值Z(1)，Z(2)…Z(K-1)Z(K)全部计算出来。

(5)将状态预测生成模块(StatePredicteEquationModule)用并行的多个独 立的子模块(SPESubModule)来实现：状态预测生成子模块1(SPESubModule1)， 计算X_pre(1):X_pre(1)＝系统状态预测初始值。状态预测生成子模块2 (SPESubModule2)，计算X_pre(2):X_pre(2)＝A*Xkf(1)。状态预测生成子模块 N-1(SPESubModuleN-1)，计算X_pre(N-1):X_pre(N-1)＝A*Xkf(N-2)。状态预测 生成子模块N(SPESubModuleN)，计算X_pre(N):X_pre(N)＝A*Xkf(N-1)。每一 个状态预测生成子模块都能够完成步骤(E)中所述的完整功能X_pre(k)＝A*Xkf(k-1)，这样N个状态预测生成子模块就可以同时计算出N个不 同的状态预测值X_pre(N)。当N＝K时，可以用N个子模块中的 K个子模块直接一次将系统的K个状态预测值X_pre(1)，X_pre(2)… X_pre(K-1)X_pre(K)全部计算出来。

(6)将预测协方差生成模块(ConvariancePredicteEquationModule)用并行 的多个独立的子模块(CPESubModule)来实现：预测协方差生成子模块1 (SPESubModule1)，计算P_pre(1):P_pre(1)＝系统预测协方差初始值。预测协方 差生成子模块2(SPESubModule2)，计算P_pre(2)：P_pre(2)＝A*P(1)*A'+Q。预 测协方差生成子模块N-1(SPESubModuleN-1)，计算P_pre(N-1)： P_pre(N-1)＝A*P(N-2)*A'+Q。预测协方差生成子模块N(SPESubModuleN)，计 算P_pre(N)：P_pre(N)＝A*P(N-1)*A'+Q。每一个预测协方差生成子模块都能够 完成步骤(F)中所述的完整功能P_pre(k)＝A*P(k-1)*A'+Q，这样N个预测协方 差生成子模块就可以同时计算出N个不同的预测协方差值P_pre(N)。当N＝K时，可以用N个子模块中的K个子模块直接一次将系统的K个预测 协方差值P_pre(1)，P_pre(2)…P_pre(K-1)，P_pre(K)全部计算出来。

(7)将卡尔曼增益计算模块(KalmanGainEquationModule)用并行的多个 独立的子模块(KGESubModule)来实现：卡尔曼增益计算子模块1 (KGESubModule1)，计算Kg(1):Kg(1)＝卡尔曼增益的初始值。卡尔曼增益计算 子模块2(KGESubModule2)，计算Kg(2)：Kg(2)＝P_pre(2)*H'*inv(H*P_pre(2)*H'+R)。卡尔曼增益计算子模块N-1 (KGESubModuleN-1)，计算Kg(N-1)： Kg(N-1)＝P_pre(N-1))*H'*inv(H*P_pre(N-1))*H'+R)。卡尔曼增益计算子模块N (KGESubModuleN)，计算Kg(N)： Kg(N)＝P_pre(N))*H'*inv(H*P_pre(N))*H'+R)。每一个卡尔曼增益计算子模块都 能够完成步骤(G)中所述的完整功能Kg(k)＝P_pre(k)*H'*inv(H*P_pre(k)*H'+R)， 这样N个卡尔曼增益计算子模块就可以同时计算出N个不同的卡尔曼增益值 Kg(N)。当N＝K时， 可以用N个子模块中的K个子模块直接一次将系统的K个卡尔曼增益值Kg(1)， Kg(2)…Kg(K-1)，Kg(K)全部计算出来。

(8)将状态最优估算值计算模块(StateUpdateEquationModule)用并行的 多个独立的子模块(SUESubModule)来实现：状态最优估算值计算子模块1 (KGESubModule1)，计算Xkf(1):Xkf(1)＝状态最优化估算值初始值。状态最优 估算值计算子模块2(KGESubModule2)，计算Xkf(2)： Xkf(2)＝X_pre(2)+Kg(2)*(Z(2)-H*X_pre(2)。状态最优估算值计算子模块N-1 (KGESubModuleN-1)，计算Xkf(N-1)： Xkf(N-1)＝X_pre(N-1)+Kg(N-1)*(Z(N-1)-H*X_pre(N-1)。状态最优估算值计算子 模块N(KGESubModuleN)，计算Xkf(N)： Xkf(N)＝X_pre(N)+Kg(N)*(Z(N)-H*X_pre(N)。每一个状态最优估算值计算子模 块都能够完成步骤(H)中所述的完整功能 Xkf(k)＝X_pre(k)+Kg(k)*(Z(k)-H*X_pre(k)，这样N个状态最优估算值计算子模 块就可以同时计算出N个不同的状态最优估算值Xkf(N)。当N＝K时，可以用N个 子模块中的K个子模块直接一次将系统的K个状态最优估算值Xkf(1)，Xkf(2)… Xkf(K-1)，Xkf(K)全部计算出来。

(9)将状态协方差更新模块(ConvarianceUpdateEquationModule)用并行 的多个独立的子模块(CUESubModule)来实现：状态协方差更新子模块1 (CUESubModule1)，计算P(1):P(1)＝状态协方差初始值。状态协方差更新子模 块2(CUESubModule2)，计算P(2):P(2)＝(I-Kg(2)*H)*P_pre(2)。状态协方差更 新子模块N-1(CUESubModuleN-1)，计算 P(N-1):P(N-1)＝(I-Kg(N-1)*H)*P_pre(N-1)。状态协方差更新子模块N (CUESubModuleN)，计算P(N):P(N)＝(I-Kg(N)*H)*P_pre(N)。每一个状态协方 差更新子模块都能够完成步骤(I)中所述的完整功能 P(k)＝(I-Kg(k)*H)*P_pre(k)，这样N个状态协方差更新子模块就可以同时计算出 N个不同的状态协方差更新值P(N)。当N＝K时，可以用N个子模块中的K个子模块直接一次将系统的K 个状态协方差更新值P(1)，P(2)…P(K-1)，P(K)全部计算出来。

(10)将测量过程中的噪声生成模块(VGenerateModule)用并行的多个独 立的子模块(VGSubModule)来实现：测量过程中噪声生成子模块1 (VGSubModule1)，计算V(1)。测量过程中噪声生成子模块2(VGSubModule2)， 计算V(2)。测量过程中噪声生成子模块N-1(VGSubModuleN-1)，计算V(N-1)。 测量过程中噪声生成子模块N(VGSubModuleN)，计算V(N)。每一个测量过 程中噪声生成子模块都能够完成步骤(J)中所述的完整功能，这样N个测量 过程中噪声生成子模块就可以同时计算出N个不同的测量过程噪声值V(N)。当 N＝K时，可以用N个子模块中的K 个子模块直接一次将系统的K个测量过程噪声值V(1)，V(2)…V(K-1)，V(K) 全部计算出来。

(11)将状态转移过程中产生的噪声生成模块(WGenerateModule)用并 行的多个独立的子模块(WGSubModule)来实现：传输噪声生成子模块1 (WGSubModule1)，计算W(1)。传输噪声生成子模块2(WGSubModule2)， 计算W(2)。传输噪声生成子模块N-1(WGSubModuleN-1)，计算W(N-1)。传 输噪声生成子模块N(WGSubModuleN)，计算W(N)。每一个传输噪声生成子 模块都能够完成步骤(K)中所述的完整功能，这样N个传输噪声生成子模块 就可以同时计算出N个不同的传输噪声值W(N)。当N＝K时，可以用N个子模块中的K个子模块直接一次将系统的K 个传输噪声值W(1)，W(2)…W(K-1)，W(K)全部计算出来。

(12)过程噪声协方差生成模块(QGenerateModule)能够完成步骤(L) 中功能。

(13)测量噪声协方差生成模块(RGenerateModule)能够完成步骤(M) 中功能。

具体而言，参照图5所示，首先将图中各模块名称说明如下：

AM(AModule)：传输矩阵生成模块。HM(HModule)：观察矩阵生成模 块。SESM(StateEquationSubModule)状态转移计算子模块。OESM (ObserveEquationSubModule)：观察测量计算子模块。SPESM (StatePredicteEquationSubModule)：状态预测生成子模块。CPESM (ConvariancePredicteEquationSubModule)：预测协方差生成子模块。KGESM(KalmanGainEquationSubModule)：卡尔曼增益计算子模块。SUESM(StateUpdateEquationSubModule)：状态最优估算值计算子模块。CUESM (ConvarianceUpdateEquationSubModule)：状态协方差更新子模块。VGSM (VGenerateSubModule)：测量噪声生成子模块。WGSM (WGenerateSubModule)：传输噪声生成子模块。RGM(RGenerateModule)： 过程噪声协方差生成模块。QGM(QGenerateModule)：测量噪声协方差生成模 块。

针对于上述的各模块或子模块的说明均是就其所完成的功能而言，至于上 述的各模块或子模块实现：如果是ASIC，上述的各模块或子模块所对应就是专 用集成电路相应的功能模块；如果是多核CPU/GPU，上述的各模块或子模块所 对应的就是完成该功能所使用的一个或多个CPU/GPU核；如果是FPGA，上述 的各模块或子模块所对应的就是实现该功能的一个或多个FPGA单元(如 DSPblock、LUT、BRAM等)。

并行卡尔曼滤波子模块间数据传递及运算方法，包括以下步骤：

1)AM(AModule-传输矩阵生成模块)计算生成的传输矩阵

2)AM计算生成的传输矩阵

3)AM计算生成的传输矩阵

4)HM(HModule-观察矩阵生成模块)计算生成的观察矩阵

5)HM计算生成的观察矩阵

6)SESM1计算生成的X(1)＝系统初始状态通过路径6_1传送给SESM2， SESM2计算生成的

7)WGSM(QGenerateModule-测量噪声协方差生成模块)1计算生成的

8)SESM1计算生

9)VGSM(VGenerateSubModule-测量噪声生成子模块)1计算生成的

10)SUESM(StateUpdateEquationSubModule-状态最优估算值计算子模块) 1计算生成的

11)CUESM(ConvarianceUpdateEquationSubModule-状态协方差更新子模 块)1计算生成的

12)QGM(QGenerateModule-测量噪声协方差生成模块)计算生成的过程 噪声协方差矩阵

13)CPESM1计算生成的P_pre(1)＝系统预测协方差初始值通过路径13_1 传送给KGESM1，CPESM2计算生成的

14)RGM(RGenerateModule-过程噪声协方差生成模块)计算生成的测量协 方差R通过路径14_1，14_2，…，14_n-1，14_n分别传送给KGESM1， KGESM2，…，KGESMn-1，KGESMn。

15)SPESM1计算生成的

16)KGESM1计算生成的Kg

17)OESM1计算生成的

18)KGESM1计算生成的Kg

19)CPESM1计算生成的

20)RGM计算生成的测量噪声协方差R通过路径20_1，20_2，…，20_n-1， 20_n分别传送给VGSM1，VGSM2，…，VGSMn-1，VGSMn。

21)QGM计算生成的过程噪声协方差矩阵

在本发明一个优选的实施例中，用并行卡尔曼滤波实现20000个状态点的 载波追踪。选用多普勒频偏DetaTheta，频偏的变化率Omega0，频偏变化率的 变化率Omega1作为系统的状态量，所以我们举例的系统为三阶系统，我们取 detav＝1e-3，detaw＝1e-3，Ts(采样时间)＝1e-3，

过程噪声协方差生成模块(RGM)计 算测量噪声协方差的经验公式为R＝detaw^2/Ts^4＝1.0e+06。测量噪声生成子模 块(VGSM)计算测量噪声V＝N(0，R)。传输噪声生成子模块(WGSM)计 算传输噪声W＝N(0，Q)。设定系统状态量的初始值为X(1)＝[0 -20*e3 0]， 系统观测量的初始值为Z(1)＝[0 0 0]，系统状态协方差初始值

如上所述，在本示例中，SESMk完成X(k)＝A*X(k-1)+L*W(k-1)，表示成 矩阵形式为，

x1(k)＝a11*x1(k-1)+a12*x2(k-1)+a13*x3(k-1)+l11*w1(k-1)+l12*w2(k-1)+l13* w3(k-1)，

x2(k)＝a21*x1(k-1)+a22*x2(k-1)+a23*x3(k-1)+l21w1(k-1)+l22*w2(k-1)+l23* w3(k-1)，

x1(k)＝x1(k-1)+1.0e-03*x2(k-1)+5.0e-07*x3(k-1)+w1(k-1)，

x2(k)＝+x2(k-1)+1.0e-03*x3(k-1)+w2(k-1)，

(A)如上所述在本示例中，OESMk完成Z(k)＝H*X(k)+V(k)，表示成矩阵 形式

表示成方程形式为，

(B)如上所述在本示例中，SPESMk完成X_pre(k)＝A*Xkf(k-1)，表示成 矩阵形式为，

X_pre1(k)＝a11*Xkf1(k-1)+a12*Xkf2(k-1)+a13*Xkf3(k-1)，

X_pre2(k)＝a21*Xkf1(k-1)+a22*Xkf2(k-1)+a23*Xkf3(k-1)，

X_pre1(k)＝Xkf1(k-1)+1.0e-03*Xkf2(k-1)+5.0e-07*Xkf3(k-1)，

X_pre2(k)＝Xkf2(k-1)+1.0e-03*Xkf3(k-1)，

(C)如上所述在本示例中，CPESMk完成计算方程为 P_pre(k)＝A*P(k-1)*A'+Q，表示成矩阵形式为，

表示成方程形式为，

P_pre11(k)＝

(A11*P11(k-1)+A12*P21(k-1)+A13*P31(k-1))*A11+(A11*P12(k-1)+A12*P22(k-1)+A13*P32(k-1))*A12+(A11*P13(k-1)+A12*P23(k-1)+A13*P33(k-1))*A13+Q 11；

P_pre12(k)＝

(A11*P11(k-1)+A12*P21(k-1)+A13*P31(k-1))*A21+(A11*P12(k-1)+A12*P22(k-1)+A13*P32(k-1))*A22+(A11*P13(k-1)+A12*P23(k-1)+A13*P33(k-1))*A23+Q 12；

P_pre13(k)＝

(A11*P11(k-1)+A12*P21(k-1)+A13*P31(k-1))*A31+(A11*P12(k-1)+A12*P22(k-1)+A13*P32(k-1))*A32+(A11*P13(k-1)+A12*P23(k-1)+A13*P33(k-1))*A33+Q 13；

P_pre21(k)＝

(A21*P11(k-1)+A22*P21(k-1)+A23*P31(k-1))*A11+(A21*P12(k-1)+A22*P22(k-1)+A23*P32(k-1))*A12+(A21*P13(k-1)+A22*P23(k-1)+A23*P33(k-1))*A13+Q 21；

P_pre22(k)＝

(A21*P11(k-1)+A22*P21(k-1)+A23*P31(k-1))*A21+(A21*P12(k-1)+A22*P22(k-1)+A23*P32(k-1))*A22+(A21*P13(k-1)+A22*P23(k-1)+A23*P33(k-1))*A23+Q 22；

P_pre23(k)＝

(A21*P11(k-1)+A22*P21(k-1)+A23*P31(k-1))*A31+(A21*P12(k-1)+A22*P22(k-1)+A23*P32(k-1))*A32+(A21*P13(k-1)+A22*P23(k-1)+A23*P33(k-1))*A33+Q 23；

P_pre31(k)＝

(A31*P11(k-1)+A32*P21(k-1)+A33*P31(k-1))*A11+(A31*P12(k-1)+A32*P22(k-1)+A33*P32(k-1))*A12+(A31*P13(k-1)+A32*P23(k-1)+A33*P33(k-1))*A13+Q 31；

P_pre32(k)＝

(A31*P11(k-1)+A32*P21(k-1)+A33*P31(k-1))*A21+(A31*P12(k-1)+A32*P22(k-1)+A33*P32(k-1))*A22+(A31*P13(k-1)+A32*P23(k-1)+A33*P33(k-1))*A23+Q 32；

P_pre33(k)＝

(A31*P11(k-1)+A32*P21(k-1)+A33*P31(k-1))*A31+(A31*P12(k-1)+A32*P22(k-1)+A33*P32(k-1))*A32+(A31*P13(k-1)+A32*P23(k-1)+A33*P33(k-1))*A33+Q 33；

(D)如上所述在本示例中，KGESMk完成计算方程为 Kg(k)＝P_pre(k)*H′*inv(H*P_pre(k)*H′+R)。表示成矩阵形式为，

表示成方程形式为，令

kgd＝(P_pre11(k)*H1*H1+P_pre21(k)*H1*H2+P_pre31(k)*H1*H3+P_pre12(k )*H1*H2+P_pre22(k)*H2*H2+P_pre32(k)*H2*H3+P_pre13(k)*H1*H3+P_pre23(k )*H2*H3+P_pre33(k)*H3*H3)+R；

kgn1(k)＝P_pre11(k)*H1+P_pre12(k)*H2+P_pre13(k)*H3；

kgn2(k)＝P_pre21(k)*H1+P_pre22(k)*H2+P_pre23(k)*H3；

kgn3(k)＝P_pre31(k)*H1+P_pre32(k)*H2+P_pre33(k)*H3；

Kg1＝kgn1/kgd；

Kg2＝kgn2/kgd；

Kg3＝kgn3/kgd；

kgd＝(P_pre11(k)+P_pre21(k)*(Ts/2)+P_pre31(k)*(Ts^2/6)+P_pre12(k)*(Ts/2)+P _pre22(k)*(Ts/2)*(Ts/2)+P_pre32(k)*(Ts/2)*(Ts^2/6)+P_pre13(k)*(Ts^2/6)+P_pre23(k) *(Ts/2)*(Ts^2/6)+P_pre33(k)*(Ts^2/6)*(Ts^2/6))+(detaw^2/Ts^4)；

kgn1(k)＝P_pre11(k)+P_pre12(k)*(Ts/2)+P_pre13(k)*(Ts^2/6)；

kgn2(k)＝P_pre21(k)+P_pre22(k)*(Ts/2)+P_pre23(k)*(Ts′2/6)；

kgn3(k)＝P_pre31(k)+P_pre32(k)*(Ts/2)+P_pre33(k)*(Ts^2/6)；

Kg1＝kgn1/kgd；

Kg2＝kgn2/kgd；

Kg3＝kgn3/kgd；

kgd＝(P_pre11(k)+P_pre21(k)*(5.0e-04)+P_pre31(k)*(1.67e-07)+P_pre12(k)*( 5.0e-04)+P_pre22(k)*(5.0e-04)*(5.0e-04)+P_pre32(k)*(5.0e-04)*(1.67e-07)+P_pre13 (k)*(1.67e-07)+P_pre23(k)*(5.0e-04)*(1.67e-07)+P_pre33(k)*(1.67e-07)*(1.67e-07))+( 1.0e+06)；

kgn1(k)＝P_pre11(k)+P_pre12(k)*(5.0e-04)+P_pre13(k)*(1.67e-07)；

kgn2(k)＝P_pre21(k)+P_pre22(k)*(5.0e-04)+P_pre23(k)*(1.67e-07)；

kgn3(k)＝P_pre31(k)+P_pre32(k)*(5.0e-04)+P_pre33(k)*(1.67e-07)；

Kg1＝kgn1/kgd；

Kg2＝kgn2/kgd；

Kg3＝kgn3/kgd；

(E)如上所述在本示例中，SUESMk其计算方程为 Xkf(k)＝X_pre(k)+Kg(k)*(Z(k)-H*X_pre(k))。

表示成矩阵形式为，

表示成方程形式为，

Xkf1(k)＝X_pre1(k)+Kg1*(Z(k)-H1*X_pre1(k)+H2*X_pre2(k)+H3*X_pre3(k))；

Xkf2(k)＝X_pre2(k)+Kg2*(Z(k)-H1*X_pre1(k)+H2*X_pre2(k)+H3*X_pre3(k))；

Xkf3(k)＝X_pre3(k)+Kg3*(Z(k)-H1*X_pre1(k)+H2*X_pre2(k)+H3*X_pre3(k))；

Xkf1(k)＝X_pre1(k)+Kg1*(Z(k)-X_pre1(k)+(Ts/2)*X_pre2(k)+(Ts^2/6)*X_pre3( k))；

Xkf2(k)＝X_pre2(k)+Kg2*(Z(k)-X_pre1(k)+(Ts/2)*X_pre2(k)+(Ts^2/6)*X_pre3( k))；

Xkf3(k)＝X_pre3(k)+Kg3*(Z(k)-X_pre1(k)+(Ts/2)*X_pre2(k)+(Ts^2/6)*X_pre3( k))；

Xkf1(k)＝X_pre1(k)+Kg1*(Z(k)-X_pre1(k)+(5.0e-04)*X_pre2(k)+(1.67e-07)*X_ pre3(k))；

Xkf2(k)＝X_pre2(k)+Kg2*(Z(k)-X_pre1(k)+(5.0e-04)*X_pre2(k)+(1.67e-07)*X_ pre3(k))；

Xkf3(k)＝X_pre3(k)+Kg3*(Z(k)-X_pre1(k)+(5.0e-04)*X_pre2(k)+(1.67e-07)*X_ pre3(k))；

(F)如上所述在本示例中，CUESMk其计算方程为 P(k)＝(I-Kg(k)*H)*P_pre(k)。表示成矩阵形式为，

表示成方程形式为，

P11(k)＝(-1)*((Kg1*H1-1)*P_pre11(k)+Kg1*H2*P_pre21(k)+Kg1*H3*P_pre31(k)) ；

P12(k)＝(-1)*((Kg1*H1-1)*P_pre12(k)+Kg1*H2*P_pre22(k)+Kg1*H3*P_pre3 2(k))；

P13(k)＝(-1)*((Kg1*H1-1)*P_pre13(k)+Kg1*H2*P_pre23(k)+Kg1*H3*P_pre3 3(k))；

P21(k)＝(-1)*(Kg2*H1*P_pre11(k)+(Kg2*H2-1)*P_pre21(k)+Kg2*H3*P_pre3 1(k))；

P22(k)＝(-1)*(Kg2*H1*P_pre12(k)+(Kg2*H2-1)*P_pre22(k)+Kg2*H3*P_pre3 2(k))；

P23(k)＝(-1)*(Kg2*H1*P_pre13(k)+(Kg2*H2-1)*P_pre23(k)+Kg2*H3*P_pre3 3(k))；

P31(k)＝(-1)*(Kg3*H1*P_pre11(k)+Kg3*H2*P_pre21(k)+(Kg3*H3-1)*P_pre3 1(k))；

P32(k)＝(-1)*(Kg3*H1*P_pre12(k)+Kg3*H2*P_pre22(k)+(Kg3*H3-1)*P_pre3 2(k))；

P33(k)＝(-1)*(Kg3*H1*P_pre13(k)+Kg3*H2*P_pre23(k)+(Kg3*H3-1)*P_pre3 3(k))；

P11(k)＝(-1)*((Kg1-1)*P_pre11(k)+Kg1*(Ts/2)*P_pre21(k)+Kg1*(Ts^2/6)*P_pre 31(k))；

P12(k)＝(-1)*((Kg1-1)*P_pre12(k)+Kg1*(Ts/2)*P_pre22(k)+Kg1*(Ts^2/6)*P_pre 32(k))；

P13(k)＝(-1)*((Kg1-1)*P_pre13(k)+Kg1*(Ts/2)*P_pre23(k)+Kg1*(Ts^2/6)*P_pre 33(k))；

P21(k)＝(-1)*(Kg2*P_pre11(k)+(Kg2*(Ts/2)-1)*P_pre21(k)+Kg2*(Ts^2/6)*P_pre 31(k))；

P22(k)＝(-1)*(Kg2*P_pre12(k)+(Kg2*(Ts/2)-1)*P_pre22(k)+Kg2*(Ts^2/6)*P_pre 32(k))；

P23(k)＝(-1)*(Kg2*P_pre13(k)+(Kg2*(Ts/2)-1)*P_pre23(k)+Kg2*(Ts^2/6)*P_pre 33(k))；

P31(k)＝(-1)*(Kg3*P_pre11(k)+Kg3*(Ts/2)*P_pre21(k)+(Kg3*(Ts^2/6)-1)*P_pre 31(k))；

P32(k)＝(-1)*(Kg3*P_pre12(k)+Kg3*(Ts/2)*P_pre22(k)+(Kg3*(Ts^2/6)-1)*P_pre 32(k))；

P33(k)＝(-1)*(Kg3*P_pre13(k)+Kg3*(Ts/2)*P_pre23(k)+(Kg3*(Ts^2/6)-1)*P_pre 33(k))；

P11(k)＝(-1)*((Kg1-1)*P_pre11(k)+Kg1*(5.0e-04)*P_pre21(k)+Kg1*(1.67e-07)* P_pre31(k))；

P12(k)＝(-1)*((Kg1-1)*P_pre12(k)+Kg1*(5.0e-04)*P_pre22(k)+Kg1*(1.67e-07)* P_pre32(k))；

P13(k)＝(-1)*((Kg1-1)*P_pre13(k)+Kg1*(5.0e-04)*P_pre23(k)+Kg1*(1.67e-07)* P_pre33(k))；

P21(k)＝(-1)*(Kg2*P_pre11(k)+(Kg2*(5.0e-04)-1)*P_pre21(k)+Kg2*(1.67e-07)* P_pre31(k))；

P22(k)＝(-1)*(Kg2*P_pre12(k)+(Kg2*(5.0e-04)-1)*P_pre22(k)+Kg2*(1.67e-07)* P_pre32(k))；

P23(k)＝(-1)*(Kg2*P_pre13(k)+(Kg2*(5.0e-04)-1)*P_pre23(k)+Kg2*(1.67e-07)* P_pre33(k))；

P31(k)＝(-1)*(Kg3*P_pre11(k)+Kg3*(5.0e-04)*P_pre21(k)+(Kg3*(1.67e-07)-1)* P_pre31(k))；

P32(k)＝(-1)*(Kg3*P_pre12(k)+Kg3*(5.0e-04)*P_pre22(k)+(Kg3*(1.67e-07)-1)* P_pre32(k))；

P33(k)＝(-1)*(Kg3*P_pre13(k)+Kg3*(5.0e-04)*P_pre23(k)+(Kg3*(1.67e-07)-1)* P_pre33(k))；

1)完成6轮运算，即用图6所示的并行卡尔曼滤波系统产生3000个系统状 态值。

2)设定系统状态量的初始值Omega1即X(3，3000)＝80*10^(3)，状态最优估 算值的初始值Xkf(3，300)＝80*10^(3)，其他数值保持不变，完成1轮运算，即 用图6所示的并行卡尔曼滤波系统产生500个系统状态值。

3)设定系统状态量的初始值Omega1即X(3，3500)＝0，状态最优估算值的 初始值Xkf(3，3500)＝0，其他数值保持不变，完成11轮运算，即用图6所示的 并行卡尔曼滤波系统产生5500个系统状态值。

4)设定系统状态量的初始值Omega1即X(3，9000)＝-80*10^(3)，状态最优 估算值的初始值Xkf(3，3500)＝-80*10^(3)，其他数值保持不变，完成1轮运算， 即用图6所示的并行卡尔曼滤波系统产生500个系统状态值。

5)设定系统状态量的初始值Omega1即X(3，9500)＝0，状态最优估算值的 初始值Xkf(3，3500)＝0，其他数值保持不变，完成12轮运算，即用图6所示的 并行卡尔曼滤波系统产生6000个系统状态值。

6)设定系统状态量的初始值Omega1即X(3，15500)＝80*10^(3)，状态最优 估算值的初始值Xkf(3，3500)＝80*10^(3)，其他数值保持不变，完成1轮运算， 即用图6所示的并行卡尔曼滤波系统产生500个系统状态值。

7)设定系统状态量的初始值Omega1即X(3，16000)＝0，状态最优估算值的 初始值Xkf(3，3500)＝0，其他数值保持不变，完成8轮运算，即用图6所示的 并行卡尔曼滤波系统产生4000个系统状态值。

至此我们用图6所示的并行卡尔曼滤波系统实现了20000状态的载波跟踪。

将图6所示的并行卡尔曼滤波系统计算生成的20000系统状态中的多普勒 频偏DetaTheta绘制成图，即得到图7。

将图6所示的并行卡尔曼滤波系统计算生成的20000系统状态中的频偏的 变化率Omega0绘制成图，即得到图8。

将图6所示的并行卡尔曼滤波系统计算生成的20000系统状态中的频偏变 化率的变化率Omega1绘制成图，即得到图9。

将20000各系统状态中的测量错误messure_err_x(k)＝Z(k)-X(1，k)和卡尔曼 估计误差kalman_err_x(k)＝Xkf(1，k)-X(1，k)绘制成图，即得到图10。

以上所述实施例仅是为充分说明本发明而所举的较佳的实施例，本发明的 保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或 变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。