下载文档原格式
卡尔曼滤波参数卡尔曼滤波（Kalman Filter）是一种利用系统的动态模型、观测数据和概率统计方法进行状态估计的算法。
它由美国科学家Rudolf E. Kálmán于1960年提出，被广泛应用于航天、航空、导航、机器人等领域。
卡尔曼滤波是一种最优的线性滤波器，它通过考虑系统模型和测量数据的不确定性来估计系统的最优状态。
卡尔曼滤波的基本思想是利用历史数据和本次观测数据，并结合系统模型进行状态估计，并通过不确定性的协方差矩阵来表示估计值的精确度。
卡尔曼滤波的基本公式如下：1. 预测阶段：状态预测：$\hat{x}_{k|k-1} = F\hat{x}_{k-1|k-1} + Bu_{k-1}$协方差预测：$P_{k|k-1} = FP_{k-1|k-1}F^T + Q$2. 更新阶段：测量残差：$y_k = z_k - H\hat{x}_{k|k-1}$协方差残差：$S_k = HP_{k|k-1}H^T + R$卡尔曼增益：$K_k = P_{k|k-1}H^TS_k^{-1}$状态修正：$\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_ky_k$协方差修正：$P_{k|k} = (I - K_kH)P_{k|k-1}$其中，$F$为状态转移矩阵，描述系统状态从上一个时刻到当前时刻的演变关系；$\hat{x}_{k|k-1}$为对状态的先验估计；$B$为控制输入矩阵，描述外部控制对状态的影响；$u_{k-1}$为上一个时刻的控制输入；$P_{k|k-1}$为对状态估计的先验协方差矩阵；$Q$为过程噪声的协方差矩阵，描述系统模型的不确定性；$H$为观测矩阵，描述观测数据和状态之间的关系；$z_k$为当前时刻的观测数据；$R$为观测噪声的协方差矩阵，描述观测数据的不确定性；$S_k$为协方差残差；$K_k$为卡尔曼增益；$y_k$为测量残差，表示观测数据和状态估计之间的差异；$\hat{x}_{k|k}$为对状态的后验估计，是基于观测数据进行修正后的状态估计；$P_{k|k}$为对状态估计的后验协方差矩阵。
卡尔曼滤波_卡尔曼算法1.引言1.1 概述卡尔曼滤波是一种用于估计系统状态的技术，通过融合传感器测量值和系统模型的预测值，提供对系统状态的最优估计。
它的应用十分广泛，特别在导航、图像处理、机器人技术等领域中发挥着重要作用。
在现实世界中，我们往往面临着各种噪声和不确定性，这些因素会影响我们对系统状态的准确估计。
卡尔曼滤波通过动态调整系统状态的估计值，可以有效地抑制这些干扰，提供更加精确的系统状态估计。
卡尔曼滤波的核心思想是利用系统模型的预测和传感器测量值之间的线性组合，来计算系统状态的最优估计。
通过动态地更新状态估计值，卡尔曼滤波可以在对系统状态的准确估计和对传感器测量值的实时响应之间进行平衡。
卡尔曼滤波算法包括两个主要步骤：预测和更新。
在预测步骤中，通过系统模型和上一时刻的状态估计值，预测当前时刻的系统状态。
在更新步骤中，将传感器测量值与预测值进行比较，然后根据测量误差和系统不确定性的权重，计算系统状态的最优估计。
卡尔曼滤波具有很多优点，例如它对传感器噪声和系统模型误差具有鲁棒性，可以提供较为稳定的估计结果。
此外，卡尔曼滤波还可以有效地处理缺失数据和不完全的测量信息，具有较高的自适应性和实时性。
尽管卡尔曼滤波在理论上具有较好的性能，但实际应用中还需考虑诸如系统模型的准确性、测量噪声的特性等因素。
因此，在具体应用中需要根据实际情况进行算法参数的调整和优化，以提高估计的准确性和可靠性。
通过深入理解卡尔曼滤波的原理和应用，我们可以更好地应对复杂环境下的估计问题，从而在实际工程中取得更好的效果。
本文将介绍卡尔曼滤波的基本原理和算法步骤，以及其在不同领域的应用案例。
希望通过本文的阅读，读者们可以对卡尔曼滤波有一个全面的了解，并能够在实际工程中灵活运用。
1.2文章结构文章结构部分的内容可以按照以下方式编写：1.2 文章结构本文将围绕卡尔曼滤波和卡尔曼算法展开论述。
首先，我们会在引言部分对卡尔曼滤波和卡尔曼算法进行简要概述，介绍其基本原理和应用领域。
卡尔曼滤波的r、q参数（最新版）目录1.卡尔曼滤波简介2.卡尔曼滤波中的 r、q 参数含义3.r、q 参数对卡尔曼滤波效果的影响4.如何设置 r、q 参数5.实际应用案例及注意事项正文一、卡尔曼滤波简介卡尔曼滤波（Kalman filter）是一种线性最优递归滤波算法，用于估计动态系统的状态变量。
它是在维纳滤波（Wiener filter）的基础上引入了系统模型信息，从而提高了滤波效果的一种滤波方法。
卡尔曼滤波广泛应用于导航定位、机器人控制、自动驾驶等领域。
二、卡尔曼滤波中的 r、q 参数含义在卡尔曼滤波中，有两个重要的参数：r 和 q。
它们分别表示状态变量的协方差矩阵 R 和系统噪声矩阵 Q。
其中，- R（State Covariance Matrix）表示系统状态变量的不确定性，是由系统自身的噪声引起的。
它包含了状态变量的方差信息，用于描述状态变量之间的相关性。
- Q（System Noise Covariance Matrix）表示系统噪声的影响，是由外部环境因素引起的。
它包含了噪声的方差信息，用于描述噪声之间的相关性。
三、r、q 参数对卡尔曼滤波效果的影响r 和 q 参数对卡尔曼滤波效果具有重要影响。
它们分别决定了状态变量的不确定性和系统噪声的影响程度。
具体来说：- r 参数越小，表示状态变量的不确定性越小，滤波器对状态变量的估计越精确。
然而，r 参数过小可能导致滤波器过于敏感，对噪声过度响应，从而降低滤波效果。
- q 参数越小，表示系统噪声的影响越小，滤波器对噪声的抑制能力越强。
然而，q 参数过小可能导致滤波器对系统噪声的估计不足，从而降低滤波效果。
四、如何设置 r、q 参数在实际应用中，r、q 参数的设置需要根据具体情况进行调整。
一般可以通过以下方法进行设置：1.根据实际系统的噪声特性和测量误差，估计状态变量的协方差矩阵R 和系统噪声矩阵 Q。
2.结合实际应用需求，调整 r、q 参数以达到较好的滤波效果。
卡尔曼滤波器算法卡尔曼滤波器算法是一种常见的数据处理算法，它能够通过对数据进行滤波，去除噪声和干扰，提高数据质量，广泛应用于各个领域。
本文将对卡尔曼滤波器算法进行详细介绍，包括其原理、应用场景以及实现方法。
一、卡尔曼滤波器算法的原理卡尔曼滤波器算法的原理是基于贝叶斯概率理论和线性系统理论的。
其核心思想是通过对系统状态的不断测量和预测，根据预测值和实际值之间的误差来调整状态估计值，从而获得更准确的状态估计结果。
具体来说，卡尔曼滤波器算法可以分为两个步骤：预测和更新。
1. 预测步骤在预测步骤中，通过上一时刻的状态估计值和状态转移矩阵对当前时刻的状态进行预测。
状态转移矩阵是描述系统状态变化的数学模型，可以根据实际情况进行定义。
2. 更新步骤在更新步骤中，通过测量值和状态预测值之间的误差，计算出卡尔曼增益，从而根据卡尔曼增益调整状态估计值。
卡尔曼增益是一个比例系数，它的大小取决于预测误差和测量误差的比例。
二、卡尔曼滤波器算法的应用场景卡尔曼滤波器算法具有广泛的应用场景，下面列举几个常见的应用场景：1. 飞机导航系统在飞机导航系统中，卡尔曼滤波器算法可以通过对飞机的位置、速度和姿态等参数进行滤波，提高导航的准确性和精度。
2. 机器人控制系统在机器人控制系统中，卡尔曼滤波器算法可以通过对机器人的位置、速度、姿态和力量等参数进行滤波，提高机器人的控制精度和稳定性。
3. 多传感器融合系统在多传感器融合系统中，卡尔曼滤波器算法可以通过对多个传感器的数据进行滤波和融合，提高数据质量和精度。
三、卡尔曼滤波器算法的实现方法卡尔曼滤波器算法的实现方法具有一定的复杂性，下面介绍一般的实现步骤：1. 定义状态向量和状态转移矩阵根据实际情况，定义状态向量和状态转移矩阵，描述系统状态的变化规律。
2. 定义测量向量和观测矩阵根据实际情况，定义测量向量和观测矩阵，描述传感器测量数据与状态向量之间的联系。
3. 计算预测值和预测误差协方差矩阵根据状态向量、状态转移矩阵和误差协方差矩阵，计算预测值和预测误差协方差矩阵。
Kalman滤波和数字滤波一、kalman滤波卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。
采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。
它适合于实时处理和计算机运算。
其他的就不介绍了。
公式简介卡尔曼滤波主要是由5个经典公式组成：X(k|k-1)=A X(k-1|k-1)+B U(k) (1)式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。
到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的协方差还没更新。
我们用P表示协方差：P(k|k-1)=A P(k-1|k-1) A’+Q (2)式(2)中，P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应的协方差，A’表示A的转置矩阵，Q是系统过程的协方差。
式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。
现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。
结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)：X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) (3)其中Kg为卡尔曼增益(Kalman Gain)：Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) (4)到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。
但是为了要令卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的协方差：P(k|k)=（I-Kg(k) H）P(k|k-1) (5)其中I 为1的矩阵，对于单模型单测量，I=1。
当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。
Kalman滤波算法的特点：（1）由于Kalman滤波算法将被估计的信号看作在白噪声作用下一个随机线性系统的输出，并且其输入/输出关系是由状态方程和输出方程在时间域内给出的，因此这种滤波方法不仅适用于平稳随机过程的滤波，而且特别适用于非平稳或平稳马尔可夫序列或高斯-马尔可夫序列的滤波，所以其应用范围是十分广泛的。
（2）Kalman滤波算法是一种时间域滤波方法，采用状态空间描述系统。
系统的过程噪声和量测噪声并不是需要滤除的对象，它们的统计特征正是估计过程中需要利用的信息，而被估计量和观测量在不同时刻的一、二阶矩却是不必要知道的。
（3）由于Kalman滤波的基本方程是时间域内的递推形式，其计算过程是一个不断地“预测-修正”的过程，在求解时不要求存储大量数据，并且一旦观测到了新的数据，随即可以算的新的滤波值，因此这种滤波方法非常适合于实时处理、计算机实现。
（4）由于滤波器的增益矩阵与观测无关，因此它可预先离线算出，从而可以减少实时在线计算量。
在求滤波器增益矩阵时，要求一个矩阵的逆，它的阶数只取决于观测方程的维数，而该维数通常很小，这样，求逆运算是比较方便的。
另外，在求解滤波器增益的过程中，随时可以算出滤波器的精度指标P，其对角线上的元素就是滤波误差向量各分量的方差。
Kalman滤波的应用领域一般地，只要跟时间序列和高斯白噪声有关或者能建立类似的模型的系统，都可以利用Kalman滤波来处理噪声问题，都可以用其来预测、滤波。
Kalman滤波主要应用领域有以下几个方面。
（1）导航制导、目标定位和跟踪领域。
（2）通信与信号处理、数字图像处理、语音信号处理。
（3）天气预报、地震预报。
（4）地质勘探、矿物开采。
（5）故障诊断、检测。
（6）证券股票市场预测。
具体事例：（1）Kalman滤波在温度测量中的应用；（2）Kalman滤波在自由落体运动目标跟踪中的应用；（3）Kalman滤波在船舶GPS导航定位系统中的应用；（4）Kalman滤波在石油地震勘探中的应用；（5）Kalman滤波在视频图像目标跟踪中的应用；。
卡尔曼滤波器的五个公式
卡尔曼滤波器（Kalman Filter）的五个公式如下：
1. 预测状态：
x̂_k = F_k * x̂_k-1 + B_k * u_k
其中，x̂_k为当前时刻k的状态估计值，F_k为状态转移矩阵，x̂_k-1为上一时刻k-1的状态估计值，B_k为外部输入矩阵，u_k为外部输入。
2. 预测误差协方差：
P_k = F_k * P_k-1 * F_k^T + Q_k
其中，P_k为当前时刻k的状态估计误差协方差矩阵，P_k-1为上一时刻k-1的状态估计误差协方差矩阵，Q_k为系统过程噪声的协方差矩阵。
3. 计算卡尔曼增益：
K_k = P_k * H_k^T * (H_k * P_k * H_k^T + R_k)^-1
其中，K_k为当前时刻k的卡尔曼增益矩阵，H_k为观测矩阵，R_k为观测噪声的协方差矩阵。
4. 更新状态估计值：
x̂_k = x̂_k + K_k * (z_k - H_k * x̂_k)
其中，z_k为当前时刻k的观测值。
5. 更新状态估计误差协方差：
P_k = (I - K_k * H_k) * P_k
其中，I为单位矩阵。
卡尔曼滤波计算举例⏹计算举例⏹卡尔曼滤波器特性假设有一个标量系统，信号与观测模型为[1][][]x k ax k n k +=+[][][]z k x k w k =+其中a 为常数，n [k ]和w [k ]是不相关的零均值白噪声，方差分别为和。
系统的起始变量x [0]为随机变量，其均值为零，方差为。
2nσ2σ[0]x P （1）求估计x [k ]的卡尔曼滤波算法；（2）当时的卡尔曼滤波增益和滤波误差方差。
220.9,1,10,[0]10nx a P =σ=σ==1. 计算举例根据卡尔曼算法，预测方程为:ˆˆ[/1][1/1]xk k ax k k -=--预测误差方差为:22[/1][1/1]x x nP k k a P k k -=--+σ卡尔曼增益为:()1222222[][/1][/1][1/1][1/1]x x x nx n K k P k k P k k a P k k a P k k -=--+σ--+σ=--+σ+σˆˆˆ[/][/1][]([][/1])ˆˆ[1/1][]([][1/1])ˆ(1[])[1/1][][]xk k x k k K k z k x k k axk k K k z k ax k k a K k xk k K k z k =-+--=--+---=---+滤波方程:()()2222222222222[/](1[])[/1][1/1]1[1/1][1/1][1/1][1/1]x x x nx n x n x nx nP k k K k P k k a P k k a P k k a P k k a P k k a P k k =--⎛⎫--+σ=---+σ ⎪--+σ+σ⎝⎭σ--+σ=--+σ+σ滤波误差方差起始：ˆ[0/0]0x=[0/0][0]x x P P =k [/1]x P k k -[/]x P k k []K k 012345689104.76443.27012.67342.27652.21422.18362.16832.16089.104.85923.64883.16542.94752.84402.79352.76870.47360.32700.26730.24040.22770.22140.21840.2168ˆ[0/0]0x=[0/0]10x P =220.9110na =σ=σ=2. 卡尔曼滤波器的特性从以上计算公式和计算结果可以看出卡尔曼滤波器的一些特性：（1）滤波误差方差的上限取决于测量噪声的方差，即()2222222[1/1][/][1/1]x nx x na P k k P k k a P k k σ--+σ=≤σ--+σ+σ2[/]x P k k ≤σ这是因为（2）预测误差方差总是大于等于扰动噪声的方差，即2[/1]x nP k k -≥σ这是因为222[/1][1/1]x x n nP k k a P k k -=--+σ≥σ（3）卡尔曼增益满足，随着k 的增加趋于一个稳定值。
Kalman 滤波算法姓名：刘金强专业：控制理论与控制工程学号：2007255◆实验目的：（1）、掌握klman 滤波实现的原理和方法（2）、掌握状态向量预测公式的实现过程（3）、了解Riccati 差分方程实现的过程和新息的基本性质和过程的计算 ◆实验要求：问题：F=[a1,a2,a3],其中a1=[1.0 0 0]的转置，a2=[0.3 1.0 0]的转置，a3=[0.1 0.2 0.4]的转置，x(0)=[3,-1,2]的转置；C=[b1,b2,b3],其中b1=[0.3 0.5]的转置，b2=[1,0.4]的转置，b3=[0.8 -0.7]的转置；V1(n)=[0 0 n1(n)sin(0.1n)]的转置，V2(n)=[n2(n) n3(n)];n1(n)为均值为零，方差为1的均匀分布白噪声;n2(n),n3(n)为均值为0，方差为0.1的均匀分布白噪声,n1(n),n2(n),n3(n)相互独立，试用卡尔曼滤波器算法估计x^(n).◆实验原理：初始条件：1ˆ(1)x=E{x(1)} K(1,0)=E{[x(1)- (1)x ][x(1)- (1)H x ]},其中(1)x =E{x(1)}输入观测向量过程：观测向量序列={y(1),…………y(n)}已知参数：状态转移矩阵F(n+1,n)观测矩阵C(n)过程噪声向量的相关矩阵1()Q n观测噪声向量的相关矩阵2()Q n计算：n=1,2,3,……………….G(n)=F(n+1,n)K(n,n+1) ()H C n 12[()(,1)()()]H C n K n n C n Q n --+Kalman 滤波器是一种线性的离散时间有限维系统。
Kalman 滤波器的估计性能是：它使滤波后的状态估计误差的相关矩阵P(n)的迹最小化。
这意味着，kalman 滤波器是状态向量x(n)的线性最小方差估计。
◆实验结果：◆程序代码：（1）主程序/******************************************************************** 问题：F=[a1,a2,a3],其中a1=[1.0 0 0]的转置，a2=[0.3 1.0 0]的转置，a3=[0.1 0.2 0.4]的转置，x(0)=[3,-1,2]的转置；C=[b1,b2,b3],其中b1=[0.3 0.5]的转置，b2=[1,0.4]的转置，b3=[0.8 -0.7]的转置；V1(n)=[0 0 n1(n)sin(0.1n)]的转置，V2(n)=[n2(n) n3(n)];n1(n)为均值为零，方差为1的均匀分布白噪声;n2(n),n3(n)为均值为0，方差为0.1的均匀分布白噪声,n1(n),n2(n),n3(n)相互独立，试用卡尔曼滤波器算法估计x^(n).********************************************************************* 设计作者：刘金强设计时间：2008年11月22日作者专业：控制理论与控制工程作者学号：2007255********************************************************************/ #include <iostream.h>#include "MathMatrix.h"#include <math.h>#include <string.h>void GaussInverse(double **a, int n);#define NUM 80 //最大序列数void main(){CMathMatrix MM;int i;int j, w;double F[3][3] = {1.0,0.3,0.1, 0, 0.1,0.2,0, 0, 0.4}; //状态转移矩阵double F_1[3][3] = {1.0,0.3,0.1,0, 0.1,0.2,0, 0, 0.4}; //状态转移矩阵的逆double **FN = new double * [3]; //申请内存空间for (i = 0; i < 3; i++){FN[i] = new double [3]; //开辟一个二维数组}for (i = 0; i < 3; i++){for (j = 0; j < 3; j++){FN[i][j] = F_1[i][j];}}GaussInverse(FN, 3);for (i = 0; i < 3; i++){for (j = 0; j < 3; j++){F_1[i][j] = FN[i][j];}}double F_T[3][3] = {1.0, 0, 0,0.3,1.0, 0,0.1,0.2,0.4}; //状态转移矩阵的转置double x[3] = {3,-1,2}; //状态向量double x1[3] = {0,0,0}; //状态预测估计向量double C[2][3] = {0.3, 1, 0.8,0.5,0.4,-0.7}; //观测矩阵double C_T[3][2] = {0.3,0.5,1, 0.4, 0.8,-0.7}; //观测矩阵的转置double alpha[2]={0}; //新息double v1[3]={0}; //过程噪声double Q1[3][3]={0}; //过程噪声相关矩阵double v2[2]={0}; //观测噪声double Q2[2][2]={0}; //观测噪声相关矩阵double y[NUM][2]={0}; //观测值序列double xx[3][NUM+1]={0}; //状态向量序列double K[3][3]={1,0,0,0,1,0,0,0,1}; //误差相关矩阵double G[3][2]={0}; //卡尔曼增益double P[3][3]={0}; //滤波状态向量的误差向量double n[NUM*2]={0};double n1[NUM]={0}; //噪声序列n1,n2,n3double n2[NUM]={0};double n3[NUM]={0};double M_3x1[3]={0};double M_1x3[3]={0};double M_3x3[3][3]={0};double M1_3x3[3][3]={0};double M_3x2[3][2]={0};double M_2x3[2][3]={0};double M_2x2[2][2]={0};double **MMid = new double *[2];for (i = 0; i < 2; i++){MMid[i] = new double [2];}double M_2x1[2]={0};double real_value[3][NUM]={0};double predict_value[3][NUM]={0};MM.random(NUM,n1,0,1); //产生高斯白噪声MM.random(NUM*2,n,0,0.2); //产生高斯白噪声memcpy(&n2[1],n,(NUM-1)*8);memcpy(&n3[1],n+NUM-1,(NUM-1)*8);real_value[0][1] = x[0];real_value[1][1] = x[1];real_value[2][1] = x[2];for(i=1;i<NUM;i++) //生成观测序列y{v2[0] = n2[i];v2[1] = n3[i];v1[0] = 0;v1[1] = 0;v1[2] = n1[i]*sin(0.1*(double)i);real_value[0][i] = x[0];real_value[1][i] = x[1];real_value[2][i] = x[2];MM.Maxtrix_multiply(&C[0][0],2,3,&x[0],3,1,&M_2x1[0]);MM.Maxtrix_add(&v2[0],&M_2x1[0],2,1);MM.Maxtrix_add(&M_2x1[0],&y[i][0],2,1);MM.Maxtrix_multiply(&F[0][0],3,3,&x[0],3,1,&M_3x1[0]);MM.Maxtrix_add(&v1[0],&M_3x1[0],3,1);x[0]=0;x[1]=0;x[2]=0;MM.Maxtrix_add(&M_3x1[0],&x[0],3,1);}for(i=1;i<NUM;i++){//计算G(n)MM.Maxtrix_multiply(&F[0][0],3,3,&K[0][0],3,3,&M_3x3[0][0]);MM.Maxtrix_multiply(&M_3x3[0][0],3,3,&C_T[0][0],3,2,&M_3x2[0][0]) ;MM.Maxtrix_multiply(&C[0][0],2,3,&K[0][0],3,3,&M_2x3[0][0]);MM.Maxtrix_multiply(&M_2x3[0][0],2,3,&C_T[0][0],3,2,&M_2x2[0][0]) ;v2[0] = n2[i];v2[1] = n3[i];MM.Maxtrix_multiply(&v2[0],2,1,&v2[0],1,2,&Q2[0][0]);MM.Maxtrix_add(&Q2[0][0],&M_2x2[0][0],2,2);MMid[0][0] = M_2x2[0][0];MMid[0][1] = M_2x2[0][1];MMid[1][0] = M_2x2[1][0];MMid[1][1] = M_2x2[1][1];GaussInverse(MMid,2);M_2x2[0][0] = MMid[0][0];M_2x2[0][1] = MMid[0][1];M_2x2[1][0] = MMid[1][0];M_2x2[1][1] = MMid[1][1];MM.Maxtrix_multiply(&M_3x2[0][0],3,2,&M_2x2[0][0],2,2,&G[0][0]);//计算α(n)MM.Maxtrix_multiply(&C[0][0],2,3,&x1[0],3,1,&M_2x1[0]);MM.Maxtrix_sub(&y[i][0],&M_2x1[0],&alpha[0],2,1);//估计x1predict_value[0][i] = x1[0];predict_value[1][i] = x1[1];predict_value[2][i] = x1[2];MM.Maxtrix_multiply(&F[0][0],3,3,&x1[0],3,1,&M_3x1[0]);MM.Maxtrix_multiply(&G[0][0],3,2,&alpha[0],2,1,&x1[0]);MM.Maxtrix_add(&M_3x1[0],&x1[0],3,1);cout<<"估计第"<<i<<"次的值为："<<endl;cout<<x[0]<<" "<<x1[0]<<endl<<x[1]<<" "<<x1[1]<<endl<<x[2]<<" "<<x1[2]<<endl;//计算P(n)MM.Maxtrix_multiply(&F_1[0][0],3,3,&G[0][0],3,2,&M_3x2[0][0]); MM.Maxtrix_multiply(&M_3x2[0][0],3,2,&C[0][0],2,3,&M_3x3[0][0]);MM.Maxtrix_multiply(&M_3x3[0][0],3,3,&K[0][0],3,3,&M1_3x3[0][0]);MM.Maxtrix_sub(&K[0][0],&M1_3x3[0][0],&P[0][0],3,3);//生成Q1(n)v1[0] = 0;v1[1] = 0;v1[2] = n1[i]*sin(0.1*(double)i);MM.Maxtrix_multiply(&v1[0],3,1,&v1[0],1,3,&Q1[0][0]);//计算K(n)MM.Maxtrix_multiply(&F[0][0],3,3,&P[0][0],3,3,&M_3x3[0][0]);MM.Maxtrix_multiply(&M_3x3[0][0],3,3,&F_T[0][0],3,3,&K[0][0]);MM.Maxtrix_add(&Q1[0][0],&K[0][0],3,3);}//释放内存for (i= 0; i < 3; i++){delete [] FN[i];}delete [] FN;for (i = 0; i < 2; i++){delete [] MMid[i];}delete [] MMid;}（2）CMathMatrix文件的源文件CMathMatrix.cpp：// mathMatrix.cpp: implementation of the CmathMatrix class./////////////////////////////////////////////////////////////////// #include "mathMatrix.h"#include <time.h>#include <math.h>#include <stdio.h>#include <stdlib.h>#include <iostream.h>#define SWAP(a,b) {temp=(a);(a)=(b);(b)=temp;}/////////////////////////////////////////////////////////////////// // Construction/Destruction/////////////////////////////////////////////////////////////////// CMathMatrix::CMathMatrix(){}CMathMatrix::~CMathMatrix(){}//矩阵求逆int *ivector(long nl, long nh);void nrerror(char error_text[]);void free_ivector(int *v, long nl, long nh);void GaussInverse(double **a, int n){int *indxc, *indxr, *ipiv;int i, icol, irow, j, k, l, ll;double big, dum, pivinv, temp;indxc = ivector(1,n);indxr = ivector(1,n);ipiv = ivector(1,n);for ( j = 1; j <= n; j++ ) ipiv[j] = 0;for ( i = 1; i <= n; i++ ){big = 0.0;for (j = 1; j <= n; j++)if (ipiv[j] != 1)for (k = 1;k <= n; k++){if (ipiv[k] == 0){if (fabs(a[j-1][k-1]) >= big){big = fabs(a[j-1][k-1]);irow = j;icol = k;}}else if ( ipiv[k] > 1)nrerror("gaussj: Singular Matrix-1");}++(ipiv[icol]);if ( irow != icol ){for ( l = 1; l <= n; l++ )SWAP(a[irow-1][l-1],a[icol-1][l-1]);}indxr[i] = irow;indxc[i] = icol;if (a[icol-1][icol-1] == 0.0)nrerror("gaussj: Singular Matrix-2");pivinv = 1.0/a[icol-1][icol-1];a[icol-1][icol-1] = 1.0;for (l = 1; l <= n; l++)a[icol-1][l-1] *= pivinv;for (ll = 1; ll <= n; ll++)if ( ll != icol ){dum = a[ll-1][icol-1];a[ll-1][icol-1] = 0.0;for ( l = 1; l <= n; l++)a[ll-1][l-1] -= a[icol-1][l-1] * dum;}}for (l = n; l >= 1; l--){if (indxr[l] != indxc[l])for (k = 1; k <= n; k++)SWAP(a[k-1][indxr[l]-1], a[k-1][indxc[l]-1]);}free_ivector(ipiv,1,n);free_ivector(indxr,1,n);free_ivector(indxc,1,n);}#undef SWAP#undef NRANSI//生成高斯白噪声void CMathMatrix::random(int n, double *e, float mean, float var) {long j;float a, b, tt;srand((unsigned)time(NULL));for(j = 0; j < n; j++){tt = rand();if((tt - 0.000001) < 0){j--;continue;}a = sqrt(-2.0 * log(tt / 32767.0));b = 2 * 3. * rand() / 32767.0;e[j] = var * a * cos(b) + mean;}}//矩阵相加void CMathMatrix::Maxtrix_add(double *Matrix_A, double *Matrix_B, unsigned int row, unsigned int col){unsigned int i,j;for(i=0;i<row;i++)for(j=0;j<col;j++)Matrix_B[i*col+j] += Matrix_A[i*col+j];}//矩阵相减void CMathMatrix::Maxtrix_sub(double *Matrix_A, double *Matrix_B, double *Matrix_C, unsigned int row, unsigned int col){unsigned int i,j;for(i=0;i<row;i++)for(j=0;j<col;j++)Matrix_C[i*col+j] = Matrix_A[i*col+j]-Matrix_B[i*col+j];}//矩阵相乘void CMathMatrix::Maxtrix_multiply(double *Matrix_A, unsigned intA_row, unsigned int A_col, double *Matrix_B, unsigned int B_row, unsigned int B_col, double *Matrix_out){unsigned int i,j,k;double sum = 0;for(i=0;i<A_row;i++)for(j=0;j<B_col;j++){sum = 0;for(k=0;k<A_col;k++)sum+=Matrix_A[i*A_col+k]*Matrix_B[k*B_col+j];Matrix_out[i*B_col+j] = sum;}}(3)、CMathMatrix文件的头文件CMathMatrix.h:// MathMatrix.h: interface for the CMathMatrix class./////////////////////////////////////////////////////////////////////// /#if !defined(AFX_MATHMATRIX_H__718FA3C9_4408_4B30_BA72_F9C28742A8F5__ INCLUDED_)#defineAFX_MATHMATRIX_H__718FA3C9_4408_4B30_BA72_F9C28742A8F5__INCLUDED_#if _MSC_VER > 1000#pragma once#endif // _MSC_VER > 1000class CMathMatrix{public://生成高斯白噪声void random(int n, double *e, float mean, float var);//矩阵相加void CMathMatrix::Maxtrix_add(double *Matrix_A, double *Matrix_B, unsigned int row, unsigned int col);//矩阵相减void CMathMatrix::Maxtrix_sub(double *Matrix_A, double *Matrix_B, double *Matrix_C, unsigned int row, unsigned int col);//矩阵相乘void CMathMatrix::Maxtrix_multiply(double *Matrix_A, unsigned int A_row, unsigned int A_col, double *Matrix_B, unsigned int B_row, unsigned int B_col, double *Matrix_out);//矩阵求逆void GaussInverse(double **a, int n);//默认构造函数与析构函数CMathMatrix();virtual ~CMathMatrix();};#endif// !defined(AFX_MATHMATRIX_H__718FA3C9_4408_4B30_BA72_F9C28742A8F5__I NCLUDED_)备注：矩阵高斯求逆矩阵的函数GaussInverse的nrutil.cpp文件和nrutil.h文档来源为:从网络收集整理.word版本可编辑.欢迎下载支持. 文件在此省略。
