[发明专利]基于FPGA的无迹卡尔曼滤波系统及并行实现方法

权利要求书

1.一种基于FPGA的无迹卡尔曼滤波系统，包括：

协方差矩阵Cholesky分解模块A，用于对分块对角协方差矩阵的对角线上的各子矩 阵分别进行Cholesky分解，得到下三角矩阵，将下三角矩阵的L个行向量，分成K组， 分别输入到Sigma点产生模块，其中L＝2m+n，m为状态量的维数，n为观测量的维数， K≥1；

Sigma点产生模块B，用于接收上一时刻的状态估计值，利用A模块得到的结果产 生K组Sigma点，分别输入到时间更新模块，其中第一组有2L/K+1个采样点，其余各 组有2L/K个采样点；

时间更新模块C，用于把接收到的Sigma点代入系统模型的状态方程，得到K组状 态预测值输入到观测预测模块，其中分别为χ_k-1中 对应于状态量、过程噪声的分量，χ_k-1为L个向量产生的K组Sigma点， χk-1=[(χk-1x)T(χk-1w)T(χk-1v)T]T,]]>为χ_k-1中对应于观测噪声的分量；

观测预测模块D，用于把状态预测值代入观测模型的观测方程，得到观测预测 值并将观测预测值和状态预测值输入到部分均值和协方差矩 阵计算模块；

部分均值和协方差矩阵计算模块E，用于对观测预测值和状态预测值加权求和，分 别计算部分状态预测协方差矩阵、部分观测预测协方差矩阵和部分互协方差矩阵，即对 K组观测预测值y_k|k-1和K组状态预测值进行加权求和，得到K组状态预测样本点的 加权和和K组观测预测样本点的加权和分别计算部分状态预测协方差矩阵 其中对于模块E₁，j＝0，1，...，2L/K，对于其余子模块， j＝1，...，2L/K，W0(c)=λ/(L+λ)+(1-α2+β),]]>当j≠0时，Wj(c)=1/{2(L+λ)},]]> λ＝α²(L+k)-L，α＞0，β≥0，α为确定采样点的散布程度，k为影响采样点分布的 四阶矩；i＝1，2，...，K，为第i组的状态预测值，计算部分观测预测协方差矩阵其中为第i组的观测预测值，计算部分互协方差矩 阵其中并将其计算结果输入到总体均值计算模 块和总体协方差矩阵计算模块；

总体均值计算模块F，用于接收部分均值和协方差矩阵计算模块的结果，分别计算 状态预测均值和观测预测均值其中并将其计算结果输入到总体协方差矩阵计算模块和状态量估计和状态协方差矩阵估计模 块；

总体协方差矩阵计算模块G，用于接收部分均值和协方差矩阵计算模块和总体均值 计算模块的结果，分别计算状态预测协方差矩阵、观测预测协方差矩阵和互协方差矩阵， 即对K个模块E_i计算得到的部分状态预测协方差矩阵部分观测预测协方差矩阵 以及部分互协方差矩阵分别求和，得到状态预测协方差矩阵其中 Px^kx^k=Px^kx^k+(β-α2)x^k|k-1x^k|k-1T-(1-α2+β)(x^k|k-1(χk|k-1,0x(0))T+(χk|k-1,0x(0))x^k|k-1T),]]>观测预测协方差矩 阵其中Px^ky^k=Px^ky^k+(β-α2)x^k|k-1y^k|k-1T-(1-α2+β)(x^k|k-1(yk|k-1,0(0))T+(χk|k-1,0x(0))y^k|k-1T)]]>和互 协方差矩阵其中：

Py^ky^k=Py^ky^k+(β-α2)y^k|k-1y^k|k-1T-(1-α2+β)(y^k|k-1(yk|k-1,0(0))T+(yk|k-1,0(0))y^k|k-1T),]]>并将观测预测协 方差矩阵输入到观测预测协方差矩阵求逆模块，将互协方差矩阵输入到增益计算模块， 将状态预测协方差矩阵和观测预测协方差矩阵输入到状态量估计和状态协方差矩阵估计 模块；

观测预测协方差矩阵求逆模块H，用于对观测预测协方差矩阵采用奇异值分解方法 求逆，即对观测预测协方差矩阵进行奇异值分解，得到正交矩阵U和正交矩阵V以 及对角矩阵∑，对∑求逆得到∑^-1，得到逆矩阵其中并将求逆结 果输入到增益计算模块；

增益计算模块I，用于接收互协方差矩阵和观测预测协方差矩阵的逆矩阵，计算增 益，即将互协方差矩阵和观测预测协方差矩阵的逆矩阵相乘，得到增益K，其 中并将增益K输入到状态量估计和状态协方差矩阵估计模块；

状态量估计和状态协方差矩阵估计模块J，用于接收增益K、状态预测均值观测预测均值状态预测协方差矩阵观测预测协方差矩阵和当前时刻的 观测数据y_k，计算状态估计值和状态协方差矩阵P_k，其中并将状态估计值作为当前时刻的最终结果输出，将状态协方差 矩阵P_k扩维后输入到A模块。