當前位置:首頁 » 操作系統 » arduino演算法

arduino演算法

發布時間: 2025-05-24 23:23:51

『壹』 加速度計和陀螺儀融合的演算法

給你arino的卡爾曼濾波融合演算法,非原創,我只是封裝了演算法。

H文件:
/*
* KalmanFilter.h
* Non-original
* Author: x2d
* Copyright (c) 2012 China
*
*/

#ifndef KalmanFilter_h
#define KalmanFilter_h

#include <WProgram.h>

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

『貳』 arino 三軸加速度感測器可以實現跌倒檢測么

可以實現
看這個文章
跌倒檢測演算法
對跌倒檢測原理的研究主要是找到人體在跌倒過程中的加速度變化特徵。圖5給出的是加速度在不同運動過程中的變化曲線,包括(a)步行上樓、(b)步
行下樓、(c)坐下、
(d)起立。其中紅色的曲線是Y軸(垂直方向)的加速度曲線,其正常靜止狀態下應該為-1g;黑色和黃色的曲線分別是X軸(前後方向)和Z軸(左右方向)
的加速度曲線,其正常靜止狀態下應該為0g;綠色的曲線是三軸加速度的矢量和,其正常靜止狀態下應該為+1g。
www.chinake.com/t/30826/2016/0518/5027759.html

熱點內容
安卓模擬地點怎麼選擇程序 發布:2025-05-25 03:44:31 瀏覽:796
安卓魔法季最後一天會有什麼 發布:2025-05-25 03:05:31 瀏覽:344
三角形判斷c語言 發布:2025-05-25 03:05:09 瀏覽:481
轉發代理伺服器搭建 發布:2025-05-25 03:03:16 瀏覽:322
怎麼把sd卡加密 發布:2025-05-25 02:58:31 瀏覽:401
資料庫應用技術考試 發布:2025-05-25 02:48:06 瀏覽:337
編程輔導 發布:2025-05-25 02:32:22 瀏覽:670
android掃一掃源碼 發布:2025-05-25 02:18:31 瀏覽:469
linuxvnc配置 發布:2025-05-25 02:16:31 瀏覽:290
excel如何取消加密 發布:2025-05-25 02:07:35 瀏覽:933