陀螺仪和加速度的值通过卡尔曼滤波融合.求C程序

给你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

你好

直接在百度文侍扰库里就可老困旦以找尺丛到

http://wenku.baidu.com/link?url=CFgu5yXH8BgHn152C3MIpSDHeSF7jm-g5T5Od3aKEIt1Nn3ChcGmTg5RYq-gCoaff5oXjlE8p1aasQhiXxeci1rjicTQYUNxSyPTPCvl7s3

clear

N=200%取200个数

w(1)=0

w=randn(1,N)%产生一个1×N的行向量，第一个数为0，w为过程噪声（其和后边的v在卡尔曼理论里均为高斯白噪声）

x(1)=0%状态x初始值

a=1%a为状态转移阵，此程序简单起见取1

for k=2:N

x(k)=a*x(k-1)+w(k-1) %系统状态方程，k时迅颤皮刻的状态等于k-1时刻状态乘以状态转移阵加噪声（此处忽略了系统的控制量）

end

V=randn(1,N)%测量噪声

q1=std(V)

Rvv=q1.^2

q2=std(x)

Rxx=q2.^2%此方程未用到Rxx

q3=std(w)

Rww=q3.^2%Rvv、Rww分别为过程噪声和测量洞山噪声的协方差(此方程只取一组数方差与协方差相同)

c=0.2

Y=c*x+V%量测方差，c为量测矩阵，同a简亩差化取为一个数

p(1)=0%初始最优化估计协方差

s(1)=0%s(1)表示为初始最优化估计

for t=2:N

p1(t)=a.^2*p(t-1)+Rww%p1为一步估计的协方差，此式从t-1时刻最优化估计s的协方差得到t-1时刻到t时刻一步估计的协方差

b(t)=c*p1(t)/(c.^2*p1(t)+Rvv)%b为卡尔曼增益，其意义表示为状态误差的协方差与量测误差的协方差之比(个人见解)

s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1))%Y(t)-a*c*s(t-1)称之为新息，是观测值与一步估计得到的观测值之差，此式由上一时刻状态的最优化估计s(t-1)得到当前时刻的最优化估计s(t)

p(t)=p1(t)-c*b(t)*p1(t)%此式由一步估计的协方差得到此时刻最优化估计的协方差

end

t=1:N

plot(t,s,'r',t,Y,'g',t,x,'b')%作图，红色为卡尔曼滤波，绿色为量测，蓝色为状态

%整体来说，此卡尔曼程序就是一个循环迭代的过程，给出初始的状态x和协方差p，得到下一时刻的x和p，循环带入可得到一系列的最优的状态估计值，此方法通常用于目标跟踪和定位。

%本人研究方向与此有关，有兴趣可以交流下。

---