c语言编程在现实生活中,C语言编程的问题

基于Kalman filter的温度估计

1.1 Background

在现实生活中有这样一个简单的例子，假设某房间的温度基本稳定，约为25度，如果想知道某时刻房间中的温度，采用的方法是利用一种测量仪器进行测量，如温度计。但由于仪器无法完全准确的测量，因此总是存在一定的误差，也就是说，无法直接获取房间温度的真实状态(事实上世间万物能够真实获知的很少很少)，利用测量与温度的关系、测量值、前一时刻的温度值，温度的变化关系，误差的分布情况等信息来推算真实的温度，这个过程就是状态估计的过程。Kalman 滤波是解决 这一问题的具有重要影响力的方法。 1.2  Kalman 滤波的数学模型    Kalman 滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。它的数学描述可以如下的通俗表示： 首先，将问题抽象成一个离散控制系统。该系统可用一个线性随机微分方程(Linear Stochastic Difference equation)来描述：X(k)=A X(k-1)+B U(k)+W(k)               (1)系统的测 量过程可以描述为：Z(k)=H X(k)+V(k)                        (2)上式中，X(k)是k时刻的系统状态(如温度)，U(k)是k时刻对系统的控制量。A和B是系统参数，如果X(k)是向量(多模)，则A和B为参数矩阵。Z(k)是k时刻的测量值(如温度计的测量值)，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程噪声(建立前后时刻的状态间的关系模型可能不那么准确)和测量的噪声(如仪器误差等)。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance分别是Q，R(这里我们假设他们不随系统状态变化而变化)。对于  满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。  (如果上述内容让你头疼，1.3可以跳过不看) 1.3  Kalman滤波的递推过程    Kalman滤波递推计算当前时刻的状态X(k)： 1.状态预估计 X(k|k-1)=A X(k-1|k-1)+B U(k)             (3) 式(3)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。 2 系统结果已经更新，但对应于X(k|k-1)的covariance还没更新。我们用P表示covariance： P(k|k-1)=A P(k-1|k-1) A’+Q (4) 式 (4)中，P(k|k-1)是X(k|k-1)对应的covariance，P(k-1|k-1)是X(k-1|k-1)对应的 covariance，A’表示A的转置矩阵，Q是系统过程的covariance。 3现在已经获得了当前状态的预测结果，然后再收集现在状态的测量值，结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)： X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) …  …    … (5) 其中Kg为卡尔曼增益(Kalman Gain)： Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) …    …… (6) 到现在为止，我们已经得到了k时刻最优的估算值X(k|k)。但是为了要使卡尔曼滤波器不断的运行下去直到系统过程结束，我 们还要更新k状态下X(k|k)的covariance： P(k|k)=(I-Kg(k) H)P(k|k-1)          …… (7)    其中I 为单位矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式(4)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。重复以上步骤就可以递推的得到任意时刻的状态的估计值。  1.4 Kalman滤波之于温度估计问题 (如果下面内容仍然让你头疼，可以只看公式(8)到(14)) 对于温度的估计问题，根据之前的描述，可以把房间的温度变化和测量看成一个系统，然后对这个系统建模，由于房间的温度与前一时刻的温度相同，而且为一 维向量，所以A=1。没有控制量，所以U(k)=0。因为测量的值是温度计的示数，跟温度直接对应，所以H=1。假设Wk和Vk都服从N(0,1)的高斯白噪声分布，因此R=1,Q=1, 温度系统简化后的模型为：        X(k)= X(k-1)+ W(k)                   (8) Z(k)= X(k)+V(k)                    (9)递推过程如下：  X(k|k-1)=X(k-1|k-1)                   (10)P(k|k-1)=P(k-1|k-1) +1          (11)X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1))           (12)  Kg(k)= P(k|k-1) / (P(k|k-1) + R)              (13) P(k|k)=(1-Kg(k))P(k|k-1)                (14)其中，X(k-1|k-1)是k-1时刻 的温度的估计值，X(k|k)是k时刻的温度的估计值，其他为计算温度估计值相关的中间变量。这样，已知X(0|0)和P(0|0)就可以递推的计算X(1|1)和P(1|1)，X(2|2)和P(2|2)，…, 即，只需令 X(k-1|k-1)=X (k|k), P(k-1|k-1)=P(k|k)即可实现递推计算X(k+1|k+1)和P(k+1|k+1)的过程。 1.5 你的任务 编写程序实现如下功能： 1假设真实的初始温度为X(0)=25(k=0时)，每一时刻的Wk和Vk都是一个服从(0,1)高斯白噪声分布的随机值，利用公式8和9设计一个函数模拟出某一时刻的的真实温度值X(k)，和温度计的测量值Z(k)。(附上生成一个服从高斯分 学可以自己搜索相关算法自己编写此函数) 2 假设X(0|0)=23，P(0|0) = 1,利用公式10-14设计一个函数能够计算k时刻的温度的估计值X(k|k)。 3 设计一个函数计算某时刻的状态估计误差e(k) = X(k|k)-X(k) 4 利用上述3个函数输出从k=1到k=50的状态估计误差 5 用多个源文件的方式来组织你的程序，如将randn()函数单独形成一个源文件，用头文件的方式引用该函数。 6 选作内容：将上述Kalman滤波的实现过程扩展使其能够处理状态X，观测Z，W，V为m维向量，A，B为m*m矩阵的情况，最后将编号的  程序抽象成一个函数KalmanFilter()。 提示：从上述递推过程可知，某一时刻的状态估计X(k|k)只与前一时刻的状态估计X(k-1|k-1)，协方差P(k|k)，当前时刻的测量Z(k)这三个变量有关，因此不需要记录所有从1时刻到k-1时刻的各个变量的值。

希望会的前辈加我QQ110065238谢谢