卡尔曼滤波算法及C语言代码

合集下载

stm32卡尔曼滤波代码

stm32卡尔曼滤波代码以下是一个使用STM32的卡尔曼滤波器的示例代码：```c#include "stm32f4xx.h"#include "math.h"#define PI 3.14159typedef struct {float x; // 状态向量float P; // 状态协方差矩阵float Q; // 过程噪声协方差float R; // 测量噪声协方差float K; // 卡尔曼增益} KalmanFilter;KalmanFilter kalmanFilter;float measuredValue; // 测量值void KalmanFilter_Init() {kalmanFilter.x = 0.0; // 初始状态为0kalmanFilter.P = 1.0; // 初始状态协方差矩阵为1 kalmanFilter.Q = 0.01; // 过程噪声协方差为0.01kalmanFilter.R = 0.1; // 测量噪声协方差为0.1kalmanFilter.K = 0.0; // 初始卡尔曼增益为0}float KalmanFilter_Update(float measurement) {// 预测步骤kalmanFilter.x = kalmanFilter.x; // 状态更新kalmanFilter.P = kalmanFilter.P + kalmanFilter.Q; // 协方差更新// 更新步骤kalmanFilter.K = kalmanFilter.P / (kalmanFilter.P + kalmanFilter.R); // 计算卡尔曼增益kalmanFilter.x = kalmanFilter.x + kalmanFilter.K * (measurement - kalmanFilter.x); // 更新状态kalmanFilter.P = (1 - kalmanFilter.K) * kalmanFilter.P; // 更新协方差return kalmanFilter.x;}int main(void) {SystemInit(); // 系统初始化KalmanFilter_Init(); // 卡尔曼滤波器初始化// 测量值示例float measurements[] = {0.5, 0.7, 0.9, 1.2, 1.4};for (int i = 0; i < sizeof(measurements) / sizeof(float); i++) {measuredValue = KalmanFilter_Update(measurements[i]);// 打印滤波后的值printf("Filtered Value: %.2f\n", measuredValue);}while (1) {// 无限循环}}```请注意，这只是一个示例代码，您可能需要根据您的具体应用进行适当的修改和调整。

c++ imu卡尔曼滤波代码

c++ imu卡尔曼滤波代码IMU卡尔曼滤波是一种常用的姿态解算方法，可以通过陀螺仪和加速度计的读数来估计姿态。

下面是一个简单的C++代码示例，用于实现IMU卡尔曼滤波。

首先，我们需要定义一些变量和矩阵。

这些变量包括姿态向量、角速度向量、加速度向量、过渡矩阵、状态向量、协方差矩阵、观测矩阵、观测向量等。

```cpptypedef Eigen::Matrix<double, 3, 1> Vector3d; // 三维向量typedef Eigen::Matrix<double, 3, 3> Matrix3d; // 三维矩阵// 姿态状态向量 [yaw pitch roll]Vector3d state;// 过渡矩阵Matrix3d F;// 状态向量Vector3d x;然后，我们需要对这些变量进行初始化。

这里我们假设初始状态为[0, 0, 0]，初始协方差为单位矩阵。

```cpp// 初始化状态向量state << 0, 0, 0;// 初始化协方差矩阵P = Matrix3d::Identity();// 初始化观测噪声协方差矩阵R << 0.01, 0, 0,0, 0.01, 0,0, 0, 0.01;```接下来，我们需要定义一个卡尔曼滤波函数，用于更新状态向量和协方差矩阵。

```cppvoid KalmanFilter(Vector3d& gyro, Vector3d& accel) {// 更新过渡矩阵F << 1, -gyro(2)*dt, gyro(1)*dt,gyro(2)*dt, 1, -gyro(0)*dt,-gyro(1)*dt, gyro(0)*dt, 1;// 更新状态向量x = F * x;// 更新协方差矩阵P = F * P * F.transpose() + Q;// 计算卡尔曼增益矩阵K = P * H.transpose() * (H * P * H.transpose() + R).inverse();在主函数中，我们需要不断读取陀螺仪和加速度计的数据，并对其进行处理。

无迹卡尔曼滤波c语言

无迹卡尔曼滤波c语言卡尔曼滤波是一种高效的递归滤波器，它使用状态方程和测量方程来估计系统的状态。

在C语言中实现卡尔曼滤波器需要定义状态变量、测量变量、状态转移矩阵、测量矩阵和过程噪声协方差矩阵等。

以下是一个简单的无迹卡尔曼滤波器的C语言实现示例：```cinclude <>include <>define Q // 过程噪声协方差矩阵define R // 测量噪声协方差矩阵typedef struct {double x; // 状态变量double P; // 状态协方差矩阵double K; // 卡尔曼增益double Q; // 过程噪声协方差矩阵double R; // 测量噪声协方差矩阵} KalmanFilter;void KalmanInit(KalmanFilter kf, double init_x, double init_P) {kf->x = init_x;kf->P = init_P;kf->K = 0;}void KalmanUpdate(KalmanFilter kf, double measurement) {// 预测步骤double P_predict = kf->P + Q; // P的预测值double K = P_predict / (P_predict + R); // 卡尔曼增益double x_predict = kf->x + K (measurement - kf->x); // x的预测值 double P_update = (1 - K) P_predict; // P的更新值// 更新状态变量和协方差矩阵kf->x = x_predict;kf->P = P_update;}int main() {KalmanFilter kf;double measurements[10] = {, , , , , , , , , }; // 测量数据double true_x = ; // 真实状态变量double init_x = true_x; // 初始状态变量值double init_P = ; // 初始协方差矩阵值KalmanInit(&kf, init_x, init_P); // 初始化卡尔曼滤波器for (int i = 0; i < 10; i++) {KalmanUpdate(&kf, measurements[i]); // 进行卡尔曼滤波更新步骤printf("State variable: %f\n", ); // 输出估计的状态变量值true_x += ; // 真实状态变量进行微小变化，模拟真实情况下的状态变化}return 0;}```。

卡尔曼滤波代码实现c++

卡尔曼滤波代码实现c++卡尔曼滤波（Kalman Filter）是一种用于估计线性系统状态的算法。

下面是一个简单的卡尔曼滤波代码实现示例，使用 C++编写：```cpp#include <iostream>#include <vector>class KalmanFilter {public:// 初始化卡尔曼滤波器KalmanFilter(double initial_state, double initial_error_variance, double process_variance) {S = initial_error_variance;X = initial_state;P = initial_error_variance;}// 预测下一时刻的状态void predict(double measurement) {X = X + measurement;P = P + process_variance;}// 修正当前时刻的状态void correct(double measurement) {S = S + P;K = P / S;X = X + K * (measurement - X);P = (1 - K) * P;}private:double X; // 当前时刻的状态估计double P; // 状态估计的误差协方差double S; // 测量噪声的协方差double K; // 卡尔曼增益double process_variance; // 过程噪声的方差};int main() {// 初始状态和误差协方差double initial_state = 0.0;double initial_error_variance = 1.0;double process_variance = 0.1;// 创建卡尔曼滤波器对象KalmanFilter filter(initial_state, initial_error_variance, process_variance);// 进行状态预测和修正std::vector<double> measurements = {1.0, 2.0, 3.0};for (double measurement : measurements) {filter.predict(measurement);filter.correct(measurement);std::cout << "当前状态估计: " << filter.X << std::endl;}return 0;}```上述代码实现了一个简单的卡尔曼滤波器，用于对线性系统的状态进行估计。

ekf卡尔曼滤波 c语言

ekf卡尔曼滤波 c语言卡尔曼滤波（Kalman Filter）是一种用于估计动态系统状态的算法，它在信号处理、控制工程和计算机视觉等领域有广泛的应用。

以下是一个简单的 EKF（Extended Kalman Filter，扩展卡尔曼滤波）卡尔曼滤波的 C 语言示例代码：```c#include <stdio.h>#include <math.h>// 定义状态向量结构体typedef struct {double x;double y;} StateVector;// 定义观测向量结构体typedef struct {double z;} ObservationVector;// 初始化卡尔曼滤波器void kalmanFilterInit(StateVector *state, double x0, double y0, double S0, double R0) {state->x = x0;state->y = y0;state->S = S0;state->R = R0;}// 预测状态void kalmanFilterPredict(StateVector *state, double u, double S) {state->x = state->x + u;state->y = state->y + u;state->S = S + S;}// 观测更新void kalmanFilterCorrect(StateVector *state, ObservationVector *observation, double H, double R) {double S = state->S + H * H;double K = state->S / S;state->x = state->x + K * (observation->z - H * state->x);state->y = state->y + K * (observation->z - H * state->y);state->S = S - K * H;}int main() {// 状态向量StateVector state;// 观测向量ObservationVector observation;// 初始状态double x0 = 0.0, y0 = 0.0, S0 = 1.0, R0 = 0.1;// 过程噪声double u = 0.1, S = 0.2;// 观测噪声double H = 1.0, R = 0.2;kalmanFilterInit(&state, x0, y0, S0, R0);for (int i = 0; i < 10; i++) {kalmanFilterPredict(&state, u, S);observation.z = H * state.x + R * (double)rand() / RAND_MAX;kalmanFilterCorrect(&state, &observation, H, R);printf("x = %f, y = %f\n", state.x, state.y);}return 0;}```这个示例代码实现了一个简单的 EKF 卡尔曼滤波。

(完整word版)卡尔曼滤波算法(C--C++两种实现代码)

卡尔曼滤波算法实现代码C++实现代码如下： ============================kalman.h================= =============== // kalman.h: interface for the kalman class. // ////////////////////////////////////////////////////////////////////// #if !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__IN CLUDED_) #define AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLU DED_#if _MSC_VER > 1000 #pragma once #endif // _MSC_VER > 1000#include <math.h> #include "cv.h"class kalman { public: void init_kalman(int x,int xv,int y,int yv); CvKalman* cvkalman; CvMat* state; CvMat* process_noise; CvMat* measurement; const CvMat* prediction; CvPoint2D32f get_predict(float x, float y);kalman(int x=0,int xv=0,int y=0,int yv=0); //virtual ~kalman();};#endif // !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C 0__INCLUDED_)============================kalman.cpp=============== =================#include "kalman.h" #include <stdio.h>/* tester de printer toutes les valeurs des vecteurs */ /* tester de changer les matrices du noises */ /* replace state by cvkalman->state_post ??? */CvRandState rng; const double T = 0.1; kalman::kalman(int x,int xv,int y,int yv) {cvkalman = cvCreateKalman( 4, 4, 0 ); state = cvCreateMat( 4, 1, CV_32FC1 ); process_noise = cvCreateMat( 4, 1, CV_32FC1 ); measurement = cvCreateMat( 4, 1, CV_32FC1 ); int code = -1;/* create matrix data */ const float A[] = { 1, T, 0, 0, 0, 1, 0, 0, 0, 0, 1, T, 0, 0, 0, 1 };const float H[] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 };const float P[] = { pow(320,2), pow(320,2)/T, 0, 0, pow(320,2)/T, pow(320,2)/pow(T,2), 0, 0, 0, 0, pow(240,2), pow(240,2)/T, 0, 0, pow(240,2)/T, pow(240,2)/pow(T,2) };const float Q[] = { pow(T,3)/3, pow(T,2)/2, 0, 0, pow(T,2)/2, T, 0, 0, 0, 0, pow(T,3)/3, pow(T,2)/2, 0, 0, pow(T,2)/2, T };const float R[] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }; cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );cvZero( measurement );cvRandSetRange( &rng, 0, 0.1, 0 ); rng.disttype = CV_RAND_NORMAL;cvRand( &rng, state );memcpy( cvkalman->transition_matrix->data.fl, A, sizeof(A)); memcpy( cvkalman->measurement_matrix->data.fl, H, sizeof(H)); memcpy( cvkalman->process_noise_cov->data.fl, Q, sizeof(Q)); memcpy( cvkalman->error_cov_post->data.fl, P, sizeof(P)); memcpy( cvkalman->measurement_noise_cov->data.fl, R, sizeof(R)); //cvSetIdentity( cvkalman->process_noise_cov, cvRealScalar(1e-5) ); //cvSetIdentity( cvkalman->error_cov_post, cvRealScalar(1)); //cvSetIdentity( cvkalman->measurement_noise_cov, cvRealScalar(1e-1) );/* choose initial state */state->data.fl[0]=x; state->data.fl[1]=xv; state->data.fl[2]=y;state->data.fl[3]=yv; cvkalman->state_post->data.fl[0]=x; cvkalman->state_post->data.fl[1]=xv; cvkalman->state_post->data.fl[2]=y; cvkalman->state_post->data.fl[3]=yv;cvRandSetRange( &rng, 0, sqrt(cvkalman->process_noise_cov->data.fl[0]), 0 ); cvRand( &rng, process_noise ); }CvPoint2D32f kalman::get_predict(float x, float y) {/* update state with current position */ state->data.fl[0]=x; state->data.fl[2]=y;/* predict point position */ /* x'k=A 鈥 k+B 鈥 kP'k=A 鈥 k-1*AT + Q */ cvRandSetRange( &rng, 0, sqrt(cvkalman->measurement_noise_cov->data.fl [0]), 0 ); cvRand( &rng, measurement );/* xk=A?xk-1+B?uk+wk */ cvMatMulAdd( cvkalman->transition_matrix, state, process_noise, cvkalman-> state_post );/* zk=H?xk+vk */cvMatMulAdd( cvkalman->measurement_matrix, cvkalman->state_post, meas urement, measurement );cvKalmanCorrect( cvkalman, measurement ); float measured_value_x = measurement->data.fl[0]; float measured_value_y = measurement->data.fl[2];const CvMat* prediction = cvKalmanPredict( cvkalman, 0 ); float predict_value_x = prediction->data.fl[0]; float predict_value_y = prediction->data.fl[2];return(cvPoint2D32f(predict_value_x,predict_value_y)); }void kalman::init_kalman(int x,int xv,int y,int yv) { state->data.fl[0]=x;state->data.fl[1]=xv; state->data.fl[2]=y; state->data.fl[3]=yv; cvkalman->state_post->data.fl[0]=x; cvkalman->state_post->data.fl[1]=xv; cvkalman->state_post->data.fl[2]=y; cvkalman->state_post->data.fl[3]=yv; }c 语言实现代码如下：#include "stdlib.h" #include "rinv.c" int lman(n,m,k,f,q,r,h,y,x,p,g) int n,m,k; double f[],q[],r[],h[],y[],x[],p[],g[]; { int i,j,kk,ii,l,jj,js; double *e,*a,*b; e=malloc(m*m*sizeof(double)); l=m; if (l<n) l=n; a=malloc(l*l*sizeof(double)); b=malloc(l*l*sizeof(double)); for (i=0; i<=n-1; i++) for (j=0; j<=n-1; j++) { ii=i*l+j; a[ii]=0.0; for (kk=0; kk<=n-1; kk++) a[ii]=a[ii]+p[i*n+kk]*f[j*n+kk]; } for (i=0; i<=n-1; i++) for (j=0; j<=n-1; j++) { ii=i*n+j; p[ii]=q[ii]; for (kk=0; kk<=n-1; kk++) p[ii]=p[ii]+f[i*n+kk]*a[kk*l+j]; } for (ii=2; ii<=k; ii++) { for (i=0; i<=n-1; i++) for (j=0; j<=m-1; j++) { jj=i*l+j; a[jj]=0.0;for (kk=0; kk<=n-1; kk++) a[jj]=a[jj]+p[i*n+kk]*h[j*n+kk];} for (i=0; i<=m-1; i++) for (j=0; j<=m-1; j++){ jj=i*m+j; e[jj]=r[jj]; for (kk=0; kk<=n-1; kk++)e[jj]=e[jj]+h[i*n+kk]*a[kk*l+j]; } js=rinv(e,m); if (js==0){ free(e); free(a); free(b); return(js);} for (i=0; i<=n-1; i++) for (j=0; j<=m-1; j++){ jj=i*m+j; g[jj]=0.0; for (kk=0; kk<=m-1; kk++)g[jj]=g[jj]+a[i*l+kk]*e[j*m+kk]; } for (i=0; i<=n-1; i++){ jj=(ii-1)*n+i; x[jj]=0.0; for (j=0; j<=n-1; j++)x[jj]=x[jj]+f[i*n+j]*x[(ii-2)*n+j]; } for (i=0; i<=m-1; i++){ jj=i*l; b[jj]=y[(ii-1)*m+i]; for (j=0; j<=n-1; j++) b[jj]=b[jj]-h[i*n+j]*x[(ii-1)*n+j]; } for (i=0; i<=n-1; i++) { jj=(ii-1)*n+i;for (j=0; j<=m-1; j++) x[jj]=x[jj]+g[i*m+j]*b[j*l];} if (ii<k){ for (i=0; i<=n-1; i++) for (j=0; j<=n-1; j++){ jj=i*l+j; a[jj]=0.0; for (kk=0; kk<=m-1; kk++)a[jj]=a[jj]-g[i*m+kk]*h[kk*n+j]; if (i==j) a[jj]=1.0+a[jj]; } for (i=0; i<=n-1; i++) for (j=0; j<=n-1; j++) { jj=i*l+j; b[jj]=0.0; for (kk=0; kk<=n-1; kk++) b[jj]=b[jj]+a[i*l+kk]*p[kk*n+j]; } for (i=0; i<=n-1; i++) for (j=0; j<=n-1; j++) { jj=i*l+j; a[jj]=0.0; for (kk=0; kk<=n-1; kk++) a[jj]=a[jj]+b[i*l+kk]*f[j*n+kk]; } for (i=0; i<=n-1; i++) for (j=0; j<=n-1; j++) { jj=i*n+j; p[jj]=q[jj]; for (kk=0; kk<=n-1; kk++) p[jj]=p[jj]+f[i*n+kk]*a[j*l+kk]; } }} free(e); free(a); free(b); return(js);}。

卡尔曼滤波c语言

卡尔曼滤波c语言卡尔曼滤波是一种用于估计系统状态的算法，它可以通过将系统测量值和先验估计值进行加权平均，得到一个更准确的状态估计值。

以下是一个简单的卡尔曼滤波的C语言实现示例：c#include <stdio.h>#define SENSOR_NOISE 0.1 测量噪声方差#define PROCESS_NOISE 0.001 过程噪声方差float kalman_filter(float measurement, float previous_estimate, float previous_covariance) {预测步骤float predicted_estimate = previous_estimate;float predicted_covariance = previous_covariance +PROCESS_NOISE;更新步骤float kalman_gain = predicted_covariance / (predicted_covariance + SENSOR_NOISE);float current_estimate = predicted_estimate + kalman_gain *(measurement - predicted_estimate);float current_covariance = (1 - kalman_gain) * predicted_covariance;return current_estimate;}int main() {float measurement1 = 10.5;float measurement2 = 12.3;float estimate = 0; 初始估计值float covariance = 1; 初始估计方差estimate = kalman_filter(measurement1, estimate, covariance);printf("Estimate after measurement1: %f\n", estimate);estimate = kalman_filter(measurement2, estimate, covariance);printf("Estimate after measurement2: %f\n", estimate);return 0;}在上述示例中，传感器测量值和初始估计值分别通过`kalman_filter`函数进行卡尔曼滤波处理。

卡尔曼滤波C程序

#include <avr/io.h>//#include <avr/delay.h>#include <avr/pgmspace.h>#include <avr/interrupt.h>#include <util/twi.h>#include <math.h>#include <stdlib.h>#include"crc16.c"#include"uart.c"#include"kalman.c"#include"twimaster.c"unsigned char Array[8]={0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08}; unsigned char Rcvbuf[2];static int i;float y1,y2;int y3=0；//输出端口初始化void PORT_initial(void){DDRB=0B00110000;PINB=0X00;PORTB=0X00;DDRD=0B01100000;PIND=0X00;PORTD=0X00;}//Timer1初始化void Timer1Init(void){TCCR1A=0;TCCR1B=_BV(WGM12)|_BV(CS11);OCR1A=0x9C40; //20ms //0x4E20; //周期是:10毫秒TIMSK1|=_BV(OCIE1A);//Timer1 CTC中断,此中断发生周期是:10毫秒ISR(TIMER1_COMPA_vect){//gyro,acc为外部变量stateUpdate(gyro);//外部函数y1=yyh1+(gyro*0.02);y2=(int)kalmanUpdate(acc);//外部函数Array[0]=acc;Array[1]=acc>>8;Array[2]=(int)y1;Array[3]=(int)y1>>8;Array[4]=y2;Array[5]=y>>8;Array[6]=y3;//Array[7]=y3>>8;CRC16(Array, Rcvbuf,8);//外部函数for(i=0;i<8;i++) USART_Transmit(Array);//外部函数 USART_Transmit(Rcvbuf[1]);//外部函数USART_Transmit(Rcvbuf[0]);//外部函数}int main(void){PORT_initial();Timer1Init();I2C_Init();USART_initial();I2C_Start(); //启动twi的中断模式。

1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。

卡尔曼滤波简介及其算法实现代码卡尔曼滤波算法实现代码（C，C＋＋分别实现）卡尔曼滤波器简介近来发现有些问题很多人都很感兴趣。

所以在这里希望能尽自己能力跟大家讨论一些力所能及的算法。

现在先讨论一下卡尔曼滤波器，如果时间和能力允许，我还希望能够写写其他的算法，例如遗传算法，傅立叶变换，数字滤波，神经网络，图像处理等等。

因为这里不能写复杂的数学公式，所以也只能形象的描述。

希望如果哪位是这方面的专家，欢迎讨论更正。

卡尔曼滤波器– Kalman Filter1．什么是卡尔曼滤波器（What is the Kalman Filter?）在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。

跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。

1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。

1957年于哥伦比亚大学获得博士学位。

我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。

如果对这编论文有兴趣，可以到这里的地址下载：/~welch/media/pdf/Kalman1960.pdf。

简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。

对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。

他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。

近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。

2．卡尔曼滤波器的介绍（Introduction to the Kalman Filter）为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。

但是，他的5条公式是其核心内容。

结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。

在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。

假设我们要研究的对象是一个房间的温度。

根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。

假设你对你的经验不是100%的相信，可能会有上下偏差几度。

我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。

另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。

我们也把这些偏差看成是高斯白噪声。

好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。

下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。

假如我们要估算k时刻的是实际温度值。

首先你要根据k-1时刻的温度值，来预测k时刻的温度。

因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟 k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。

然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。

由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。

究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的 covariance来判断。

因为Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78* (25-23)=24.56度。

可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。

现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。

到现在为止，好像还没看到什么自回归的东西出现。

对了，在进入 k+1时刻之前，我们还要算出k 时刻那个最优值（24.56度）的偏差。

算法如下：((1-Kg)*5^2)^0.5=2.35。

这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。

就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。

他运行的很快，而且它只保留了上一时刻的covariance。

上面的Kg，就是卡尔曼增益（Kalman Gain）。

他可以随不同的时刻而改变他自己的值，是不是很神奇！下面就要言归正传，讨论真正工程系统上的卡尔曼。

3．卡尔曼滤波器算法（The Kalman Filter Algorithm）在这一部分，我们就来描述源于Dr Kalman 的卡尔曼滤波器。

下面的描述，会涉及一些基本的概念知识，包括概率（Probability），随即变量（Random Variable），高斯或正态分配（Gaussian Distribution）还有State-space Model等等。

但对于卡尔曼滤波器的详细证明，这里不能一一描述。

首先，我们先要引入一个离散控制过程的系统。

该系统可用一个线性随机微分方程（Linear Stochastic Difference equation）来描述：X(k)=A X(k-1)+B U(k)+W(k)再加上系统的测量值：Z(k)=H X(k)+V(k)上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。

A和B是系统参数，对于多模型系统，他们为矩阵。

Z(k)是k时刻的测量值，H 是测量系统的参数，对于多测量系统，H为矩阵。

W(k)和V(k)分别表示过程和测量的噪声。

他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance 分别是Q，R（这里我们假设他们不随系统状态变化而变化）。

对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。

下面我们来用他们结合他们的covariances 来估算系统的最优化输出（类似上一节那个温度的例子）。

首先我们要利用系统的过程模型，来预测下一状态的系统。

假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：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)的covariance还没更新。

式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。

现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。

但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：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)。

这样，算法就可以自回归的运算下去。

卡尔曼滤波器的原理基本描述了，式子1，2，3，4和5就是他的5 个基本公式。

根据这5个公式，可以很容易的实现计算机的程序。

下面，我会用程序举一个实际运行的例子。

4．简单例子（A Simple Example）这里我们结合第二第三节，举一个非常简单的例子来说明卡尔曼滤波器的工作过程。

所举的例子是进一步描述第二节的例子，而且还会配以程序模拟结果。

根据第二节的描述，把房间看成一个系统，然后对这个系统建模。

当然，我们见的模型不需要非常地精确。

我们所知道的这个房间的温度是跟前一时刻的温度相同的，所以A=1。

没有控制量，所以U(k)=0。

因此得出：X(k|k-1)=X(k-1|k-1) (6)式子（2）可以改成：P(k|k-1)=P(k-1|k-1) +Q (7)因为测量的值是温度计的，跟温度直接对应，所以H=1。

假设房间的真实温度为25度，我模拟了200个测量值，这些测量值的平均值为25度，但是加入了标准偏差为几度的高斯白噪声（在图中为蓝线）。

为了令卡尔曼滤波器开始工作，我们需要告诉卡尔曼两个零时刻的初始值，是X(0|0)和P(0|0)。

他们的值不用太在意，随便给一个就可以了，因为随着卡尔曼的工作，X会逐渐的收敛。

但是对于P，一般不要取0，因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统最优的，从而使算法不能收敛。

我选了 X(0|0)=1度，P(0|0)=10。

该系统的真实温度为25度，图中用黑线表示。

图中红线是卡尔曼滤波器输出的最优化结果（该结果在算法中设置了Q=1e-6，R=1e-1）。

最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家Ｋолмогоров等人的研究工作，后人统称为维纳滤波理论。

从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。