Kalman 滤波的数学模型C语言编程问题

[KEST，L，P

=卡尔曼(SYS，青年毕游，护士，NN)

卡尔曼滤波器的信号模型

X(K)=

A

*

X(k-1)+

W(K)

/>

Y(K)=

C

*

X(K)+

V(K)

W和V上的两个W和V

E

{WW“

}

=

QN，这是系统噪声的协方差矩阵

E

{VV'}

=

RN，测量噪声的协方差矩阵

E

{WV'}

=

NN，祥数侍这一下应该从字面上相互系统的噪声和观测噪声的协方差矩阵

白噪声均值为0，所谨吵以上述的几个值?的自相关和互相关函数

系统给定的系统模型

给你arduino的卡尔曼滤波融合算法,我只是封装了算法.

另外你这么难的问题应该给点分才厚道啊!

H文件：

/*

* KalmanFilter.h

* Non-original

* Author:x2d

* Copyright (c) 2012 China

*

*/

#ifndef KalmanFilter_h

#define KalmanFilter_h

#include

class KalmanFilter

{

public:

KalmanFilter()

/*

卡尔曼融合计算

angle_m:加速度计测量唤液并通过atan2(ax,ay)方法计算得到的角度（弧度值）

gyro_m:陀螺仪测量的念卖角速度值（弧度值仔链逗）

dt:采样时间(s)

outAngle:卡尔曼融合计算出的角度（弧度值）

outAngleDot:卡尔曼融合计算出的角速度（弧度值）

*/

void getValue(double angle_m,double gyro_m,double dt,double &outAngle,double &outAngleDot)

private:

double C_0,Q_angle,Q_gyro,R_angle

double q_bias,angle_err,PCt_0,PCt_1,E,K_0,K_1,t_0,t_1

double angle,angle_dot

double P[2][2]

double Pdot[4]

}

CPP文件：

/*

* KalmanFilter.cpp

* Non-original

* Author:x2d

* Copyright (c) 2012 China

*

*/

#include "KalmanFilter.h"

KalmanFilter::KalmanFilter()

{

C_0 = 1.0f

Q_angle = 0.001f

Q_gyro = 0.003f

R_angle = 0.5f

q_bias = angle_err = PCt_0 = PCt_1 = E = K_0 = K_1 = t_0 = t_1 = 0.0f

angle = angle_dot = 0.0f

P[0][0] = 1.0f

P[0][1] = 0.0f

P[1][0] = 0.0f

P[1][1] = 1.0f

Pdot[0] = 0.0f

Pdot[1] = 0.0f

Pdot[2] = 0.0f

Pdot[3] = 0.0f

}

void KalmanFilter::getValue(double angle_m,double gyro_m,double dt,double &outAngle,double &outAngleDot)

{

/*

Serial.print("angle_m = ")

Serial.print(angle_m)

Serial.print("")

Serial.print("gyro_m = ")

Serial.print(gyro_m)

Serial.print("")

*/

angle+=(gyro_m-q_bias) * dt

angle_err = angle_m - angle

Pdot[0] = Q_angle - P[0][1] - P[1][0]

Pdot[1] = -P[1][1]

Pdot[2] = -P[1][1]

Pdot[3] = Q_gyro

P[0][0] += Pdot[0] * dt

P[0][1] += Pdot[1] * dt

P[1][0] += Pdot[2] * dt

P[1][1] += Pdot[3] * dt

PCt_0 = C_0 * P[0][0]

PCt_1 = C_0 * P[1][0]

E = R_angle + C_0 * PCt_0

K_0 = PCt_0 / E

K_1 = PCt_1 / E

t_0 = PCt_0

t_1 = C_0 * P[0][1]

P[0][0] -= K_0 * t_0

P[0][1] -= K_0 * t_1

P[1][0] -= K_1 * t_0

P[1][1] -= K_1 * t_1

angle += K_0 * angle_err

q_bias += K_1 * angle_err

angle_dot = gyro_m-q_bias

outAngle = angle

outAngleDot = angle_dot

/*

Serial.print("angle = ")

Serial.print(angle)

Serial.print("")

Serial.print("angle_dot = ")

Serial.print(angle_dot)

Serial.print("")

*/

}

#endif

给你“rinv.c ”

不知道码坦是不是你要的，随便找了个数值分析厅银算法里头的

#include "stdlib.h"

#include "math.h"

#include "stdio.h"迟伏桐

int rinv(n,a)

int n

double a[]

{ int *is,*js,i,j,k,l,u,v

double d,p

is=malloc(n*sizeof(int))

js=malloc(n*sizeof(int))

for (k=0k=n-1k++)

{ d=0.0

for (i=ki=n-1i++)

for (j=kj=n-1j++)

{ l=i*n+jp=fabs(a[l])

if (p>d) { d=pis[k]=ijs[k]=j}

}

if (d+1.0==1.0)

{ free(is)free(js)printf("err**not inv\n")

return(0)

}

if (is[k]!=k)

for (j=0j=n-1j++)

{ u=k*n+jv=is[k]*n+j

p=a[u]a[u]=a[v]a[v]=p

}

if (js[k]!=k)

for (i=0i=n-1i++)

{ u=i*n+kv=i*n+js[k]

p=a[u]a[u]=a[v]a[v]=p

}

l=k*n+k

a[l]=1.0/a[l]

for (j=0j=n-1j++)

if (j!=k)

{ u=k*n+ja[u]=a[u]*a[l]}

for (i=0i=n-1i++)

if (i!=k)

for (j=0j=n-1j++)

if (j!=k)

{ u=i*n+j

a[u]=a[u]-a[i*n+k]*a[k*n+j]

}

for (i=0i=n-1i++)

if (i!=k)

{ u=i*n+ka[u]=-a[u]*a[l]}

}

for (k=n-1k>=0k--)

{ if (js[k]!=k)

for (j=0j=n-1j++)

{ u=k*n+jv=js[k]*n+j

p=a[u]a[u]=a[v]a[v]=p

}

if (is[k]!=k)

for (i=0i=n-1i++)

{ u=i*n+kv=i*n+is[k]

p=a[u]a[u]=a[v]a[v]=p

}

}

free(is)free(js)

return(1)

}

---