运动状态识别（python转c）
由于算法要运行在手表上，算力和内存空间有限，需要进行内存压缩和c代码实现。
状态识别（计算步数+分段滤波）_传统处理方法
Status_detection.c
#include
#include
#include
#include
#define buf_p 7
#define EPS 0.000001
float g_Tx[250], g_Tx1[250];
float g_Sp[20], g_Tvec[20], g_Zi[20];
int g_Is[10], g_Js[10];
float b_ppg[3] = { 0.0133592,0.0267184,0.0133592 };
float a_ppg[3] = { 1, -1.64745998, 0.70089678 };//b_u和a_u为滤波器参数
void Filter(const float* x, float* y, int xlen, float* a, float* b, int nfilt, float* g_Zi)//nfilt为系数数组长度，g_Zi为延时系数
{
float tmp;
int i, j;
//判断a[0]是否为1，若不是，归一化下
if ((*a - 1.0 > EPS) || (*a - 1.0 < -EPS))
{
tmp = *a;
for (i = 0; i < nfilt; i++)
{
b[i] /= tmp;
a[i] /= tmp;
}
}
memset(y, 0, xlen * sizeof(float));//将y清零，以双浮点为单位
a[0] = 0.0;
for (i = 0; i < xlen; i++)
{
for (j = 0; i >= j && j < nfilt; j++)
{
y[i] += (b[j] * x[i - j] - a[j] * y[i - j]);
}
//滤波器阶数大于2就加上延时系数
if (g_Zi&&i < nfilt - 1) y[i] += g_Zi[i];
}
a[0] = 1.0;//还原滤波系数
}
//矩阵乘法 ，第一个矩阵列数必须要和第二个矩阵的行数相同，输出结果存放在c中
void Trmul(float *a, float *b, float *c, int m, int n, int k)
{
int i, j, l, u;
for (i = 0; i <= m - 1; i++)
for (j = 0; j <= k - 1; j++)
{
u = i * k + j; c[u] = 0.0;
for (l = 0; l <= n - 1; l++)
c[u] = c[u] + a[i*n + l] * b[l*k + j];
}
return;
}
//求逆矩阵，当返回值为0时成功，a变为逆矩阵
int Rinv(float *a, int n)
{
int i, j, k, l, u, v;
float d, p;
for (k = 0; k <= n - 1; k++)
{
d = 0.0;
for (i = k; i <= n - 1; i++)
for (j = k; j <= n - 1; j++)
{
l = i * n + j; p = fabs(a[l]);
if (p > d) { d = p; g_Is[k] = i; g_Js[k] = j; }
}
if (g_Is[k] != k)
for (j = 0; j <= n - 1; j++)
{
u = k * n + j; v = g_Is[k] * n + j;
p = a[u]; a[u] = a[v]; a[v] = p;
}
if (g_Js[k] != k)
for (i = 0; i <= n - 1; i++)
{
u = i * n + k; v = i * n + g_Js[k];
p = a[u]; a[u] = a[v]; a[v] = p;
}
l = k * n + k;
a[l] = 1.0 / a[l];
for (j = 0; j <= n - 1; j++)
if (j != k)
{
u = k * n + j; a[u] = a[u] * a[l];
}
for (i = 0; i <= n - 1; i++)
if (i != k)
for (j = 0; j <= n - 1; j++)
if (j != k)
{
u = i * n + j;
a[u] = a[u] - a[i*n + k] * a[k*n + j];
}
for (i = 0; i <= n - 1; i++)
if (i != k)
{
u = i * n + k; a[u] = -a[u] * a[l];
}
}
for (k = n - 1; k >= 0; k--)
{
if (g_Js[k] != k)
for (j = 0; j <= n - 1; j++)
{
u = k * n + j; v = g_Js[k] * n + j;
p = a[u]; a[u] = a[v]; a[v] = p;
}
if (g_Is[k] != k)
for (i = 0; i <= n - 1; i++)
{
u = i * n + k; v = i * n + g_Is[k];
p = a[u]; a[u] = a[v]; a[v] = p;
}
}
return(0);
}
int FiltFilt(float* x, float* y, int len)
{
// int nfact = 4;
int nfact = 2;
int tlen; //length of g_Tx
int i;
float *p, *t, *end;
float tmp, tmp1;
//float sum = 0.0;
//float average = 0.0;
//for (int p = 0; p < len; p++)
//{
// sum += x[p];
//}
//average = (float)sum / len;
printf("平均值 = %.2f", average);
//for (int p = 0; p < len; ++p)
//{
// x[p] = x[p] - average;
// //x2[p] = -1 * x2[p];
//}
//镜像到至少三倍长度
tlen = 6 * nfact + len;
tmp = x[0];
for (p = x + 3 * nfact, t = g_Tx; p > x; --p, ++t)
*t = 2.0*tmp - *p;
for (end = x + len; p < end; ++p, ++t)
*t = *p;
tmp = x[len - 1];
for (end = g_Tx + tlen, p -= 2; t < end; --p, ++t)
*t = 2.0*tmp - *p;
//g_Tx初始化
end = g_Sp + nfact * nfact;
p = g_Sp;
// while (p < end) *p++ = 0.0L;
while (p < end) *p++ = 0.0f;
g_Sp[0] = 1.0 + a_ppg[1];
for (i = 1; i < nfact; i++)
{
g_Sp[i*nfact] = a_ppg[i + 1];
/* g_Sp[i*nfact + i] = 1.0L;
g_Sp[(i - 1)*nfact + i] = -1.0L;*/
g_Sp[i*nfact + i] = 1.0f;
g_Sp[(i - 1)*nfact + i] = -1.0f;
}
for (i = 0; i < nfact; i++)
{
g_Tvec[i] = b_ppg[i + 1] - a_ppg[i + 1] * b_ppg[0];
}
if (Rinv(g_Sp, nfact))
{
for (i = 0; i < 50; i++)
{
g_Zi[i] = 0;
}
}
else
{
Trmul(g_Sp, g_Tvec, g_Zi, nfact, nfact, 1);
} //g_Zi延迟系数计算
//1st filter
tmp1 = g_Tx[0];
if (g_Zi)
for (p = g_Zi, end = g_Zi + nfact; p < end;) *(p++) *= tmp1;
{
//Filter(g_Tx, g_Tx1, tlen, a_ppg, b_ppg, 5, g_Zi);
Filter(g_Tx, g_Tx1, tlen, a_ppg, b_ppg, 3, g_Zi);
}
//翻转g_Tx1
for (p = g_Tx1, end = g_Tx1 + tlen - 1; p < end; p++, end--)
{
tmp = *p;
*p = *end;
*end = tmp;
}
//2nd filter
tmp1 = (*g_Tx1) / tmp1;
if (g_Zi)
for (p = g_Zi, end = g_Zi + nfact; p < end;) *(p++) *= tmp1;
{
//Filter(g_Tx1, g_Tx, tlen, a_ppg, b_ppg, 5, g_Zi);
Filter(g_Tx1, g_Tx, tlen, a_ppg, b_ppg, 3, g_Zi);
}
//output
end = y + len;
p = g_Tx + 3 * nfact + len - 1;
while (y < end)
{
*y++ = *p--;
}
return 0;
}
// 需要滤波，否则会出现两个波峰/波谷的情况（在波峰/波谷处是直线，则会出现此情况）
// 寻找波峰
short FindPeak1(float *arr, short *peak_index)
{
short n = 0;
for (short i = 1; i < 199; i++)
{
if (arr[i]>arr[i-1] && arr[i] > arr[i + 1] && n < 33)
{
peak_index[n] = i;
n += 1;
}
}
return n;
}
// 寻找波谷
short FindPeak2(float *arr, short *peak_index)
{
short n = 0;
for (short i = 1; i < 199; i++)
{
if (arr[i] < arr[i - 1] && arr[i] < arr[i + 1] && n < 33)
{
peak_index[n] = i;
n += 1;
}
}
return n;
}
// 跑步状态检测
short RunStateDetection(float arr[], float data[], short peak1_index[], short peak2_index[], float allPeak1[], short allPeak1_index[], short allPeak1_nums[],
float allPeak2[], short allPeak2_index[], short allPeak2_nums[])
{
short i = 0;
// 1.低通滤波
FiltFilt(arr, data, 200);
// 2.寻找波峰/波谷
for (i = 0; i < 33; i++)
{
peak1_index[i] = 0;
peak2_index[i] = 0;
}
short peak1_nums = FindPeak1(data, peak1_index);
short peak2_nums = FindPeak2(data, peak2_index);
// 3.纠正波峰波谷
if (allPeak1_nums[0] ==0 && allPeak2_nums[0] == 0) // 0条记录
{
// peak1
for (i = 0; i < peak1_nums; i++)
{
allPeak1[i] = data[peak1_index[i]];
allPeak1_index[i] = peak1_index[i];
}
allPeak1_nums[0] = peak1_nums;
// peak2
for (i = 0; i < peak2_nums; i++)
{
allPeak2[i] = data[peak2_index[i]];
allPeak2_index[i] = peak2_index[i];
}
allPeak2_nums[0] = peak2_nums;
}
else // 有历史记录
{
short nums;
if (allPeak1_nums[1] == 0 && allPeak2_nums[1] == 0)
{
nums = 1;
}
else if (allPeak1_nums[2] == 0 && allPeak2_nums[2] == 0)
{
nums = 2;
}
else
{
nums = 2;
// 重置peak数据
for (i = 0; i < 100; i++)
{
// peak1
allPeak1[i] = (i < allPeak1_nums[1] + allPeak1_nums[2]) ? (allPeak1[allPeak1_nums[0] + i]) : 0;
allPeak1_index[i] = (i < allPeak1_nums[1] + allPeak1_nums[2]) ? (allPeak1_index[allPeak1_nums[0] + i] - 200) : 0;
// peak2
allPeak2[i] = (i < allPeak2_nums[1] + allPeak2_nums[2]) ? (allPeak2[allPeak2_nums[0] + i]) : 0;
allPeak2_index[i] = (i < allPeak2_nums[1] + allPeak2_nums[2]) ? (allPeak2_index[allPeak2_nums[0] + i] - 200) : 0;
}
for (i = 1; i < 3; i++)
{
allPeak1_nums[i - 1] = allPeak1_nums[i];
allPeak2_nums[i - 1] = allPeak2_nums[i];
}
allPeak1_nums[2] = 0;
allPeak2_nums[2] = 0;
}
// peak1
short base_index1 = 0;
short base_index2 = 0;
for (i = 0; i < nums; i++)
{
base_index1 += allPeak1_nums[i];
base_index2 += allPeak2_nums[i];
}
if (peak1_nums > 0 && peak2_nums > 0)
{
// 如果当前数据的第一个点是波峰，历史的最后一个点也是波峰，则删除当前第一个波峰
if (peak1_index[0]allPeak2_index[base_index2 - 1])
{
for (i = 1; i < 33; i++)
{
peak1_index[i - 1] = (i < peak1_nums) ? peak1_index[i] : 0;
}
peak1_nums = peak1_nums - 1;
}
for (i = 0; i < peak1_nums; i++)
{
allPeak1[base_index1 + i] = data[peak1_index[i]];
allPeak1_index[base_index1 + i] = peak1_index[i] + 200 * nums;
}
allPeak1_nums[nums] = peak1_nums;
}
// peak2
if (peak1_nums > 0 && peak2_nums > 0)
{
// 如果当前数据的第一个点是波谷，历史的最后一个点也是波谷，则删除当前第一个波谷
if (peak1_index[0]>peak2_index[0] && allPeak1_index[base_index1 - 1] allPeak2_index[peak2_total -1]) // 结尾
{
for (i = 0; i < 100; i++)
{
allPeak1_index[i] = (i < peak1_total - 1) ? allPeak1_index[i] : 0;
allPeak1[i] = (i < peak1_total - 1) ? allPeak1[i] : 0;
}
if (allPeak1_nums[1] == 0 && allPeak2_nums[1] == 0)
{
allPeak1_nums[0] = allPeak1_nums[0] - 1;
}
else if (allPeak1_nums[2] == 0 && allPeak2_nums[2] == 0)
{
allPeak1_nums[1] = allPeak1_nums[1] - 1;
}
else
{
allPeak1_nums[2] = allPeak1_nums[2] - 1;
}
}
// 5.计算步数
short step = 0;
peak1_total = allPeak1_nums[0] + allPeak1_nums[1] + allPeak1_nums[2];
peak2_total = allPeak2_nums[0] + allPeak2_nums[1] + allPeak2_nums[2];
if (peak1_total< peak2_total)
{
for (i = 0; i < peak1_total; i++)
{
short distance1 = allPeak2_index[i + 1] - allPeak2_index[i];
short distance2 = allPeak1[i] - allPeak2[i];
if (distance1 > 25 && distance2 > 3000)
{
step += 1;
}
}
}
// 6.判断结果,2-跑步，0-非跑步
short pred=0;
pred = (step >= 5) ? 2 : 0;
return pred;
}
void main()
{
float arr[] = { 3526, 3104, 2735, 2496, 2421, 2443, 2434, 2341, 2247, 2294, 2565, 3082, 3764, 4497, 5083, 5355, 5457, 5624, 5996, 6515, 6919, 7095, 7147, 7157, 7079, 6848, 6440, 5927, 5431, 4994, 4574, 4082, 3445, 2707, 2011, 1473, 1125, 877, 657, 471, 366, 417, 688, 1129, 1691, 2405, 3044, 3444, 3728, 3617, 3596, 6522, 11834, 13959, 12571, 10776, 9181, 7522, 6433, 5821, 5226, 4994, 4928, 4797, 4640, 4284, 3752, 3082, 2433, 2075, 1797, 1849, 2122, 2317, 2417, 2472, 2730, 3188, 3670, 4428, 5278, 5771, 6141, 6547, 6697, 6637, 6633, 6645, 6598, 6353, 5951, 5614, 5379, 5089, 4650, 4119, 3573, 3058, 2640, 2381, 2223, 2048, 1857, 1683, 1519, 1413, 1438, 1703, 2164, 2686, 3216, 3803, 4583, 5426, 6212, 6935, 7361, 7461, 7473, 7445, 7429, 7485, 7357, 6849, 6105, 5352, 4722, 4172, 3661, 3242, 2910, 2629, 2403, 2285, 2248, 2200, 2082, 1934, 1881, 2082, 2567, 3205, 3923, 4644, 5200, 5628, 6095, 6578, 6887, 7054, 7265, 7379, 7255, 6996, 6710, 6420, 6116, 5762, 5366, 4919, 4343, 3642, 2953, 2381, 1974, 1713, 1564, 1492, 1447, 1456, 1640, 2070, 2715, 3473, 4266, 5095, 5915, 6695, 7528, 8251, 8488, 8230, 7778, 7348, 6950, 6546, 6019, 5344, 4692, 4182, 3766, 3363, 2952, 2598, 2376, 2245, 2160, 2090, 2035, 2041 };
// 100*2*4+136*2*2+200*4=2148字节 -100*2*4-100*2*2=948字节
float allPeak1[100] = { 0 };
short allPeak1_index[100] = { 0 };
short allPeak1_nums[3] = { 0 };
float allPeak2[100] = { 0 };
short allPeak2_index[100] = { 0 };
short allPeak2_nums[3] = { 0 };
short peak1_index[33] = { 0 };
short peak2_index[33] = { 0 };
float data[200] = { 0 };
for (short i = 0; i < 4; i++)
{
short label = RunStateDetection(arr, data, peak1_index, peak2_index, allPeak1, allPeak1_index, allPeak1_nums, allPeak2, allPeak2_index, allPeak2_nums);
printf("label: %d\n",label);
}
}
状态识别_随机森林模型
load_model.c
#include
#include "opencv2/core/core.hpp"
#include "opencv2/ml/ml.hpp"
#include
#include
#include
using namespace std;
using namespace cv;
using namespace cv::ml;
// n维vector转mat
Mat vector2Mat(vector> src)
{
int feature_nums = int(src.size()*src[0].size());
Mat test(1, feature_nums, CV_32FC1); // CV_64FC1 1*200
for (int i = 0; i < src.size(); ++i) {
for (int j = 0; j < src[0].size(); j++)
{
int index = i * src[0].size() + j;
test.at(0, index) = src[i][j];
}
}
return test;
}
// 数据输入为vector
int StateEstimate(vector> acc, vector> gyr, vector ppg, cv::Ptr model)
{
// 挑出测试数据
vector> all_data;
vector az;
for (int i = 0; i < acc.size(); i++)
{
az.push_back(acc[i][2]);
}
all_data.push_back(az);
// 数据转换，vector转mat
Mat test_data;
test_data =vector2Mat(all_data);
// 判断测试数据的特征维度是否正确，不正确返回-2
if (test_data.cols != 200)
{
return -2;
}
// 模型预测
cv::Mat res;
model->predict(test_data, res);
// 预测结果判断是否为空
if (res.empty()) {
return -1;
}
// 输出动作分类情况：-1-预测empty，-2-测试数据特征维度错误，0-非跑步，2-跑步
int action = (int)(res.at(0, 0));
if (action==2)
{
return 2;
}
return 0;
}
int main() {
定义数据
vector> acc;
for (int i = 0; i < 200; i++)
{
vectordata;
for (int j = 0; j < 3; j++)
{
data.push_back(i * 3 + j);
}
acc.push_back(data);
}
vector> gyr;
for (int i = 0; i < 200; i++)
{
vectordata;
for (int j = 0; j < 3; j++)
{
data.push_back(i * 3 + j);
}
gyr.push_back(data);
}
vector ppg;
for (int i = 0; i < 200; i++) {
ppg.push_back(i);
}
// 模型加载
cv::Ptr model = cv::ml::RTrees::load("svm-az.xml");
// 模型预测：-1-预测empty，-2-测试数据特征维度错误，0-静止 ，1-走路
int pred=StateEstimate(acc, gyr, ppg, model);
std::cout << "预测结果为:" << pred << std::endl;
}