运动状态识别（python转c）

最新推荐文章于 2023-07-04 10:33:22 发布

尼古拉斯·two_dog

最新推荐文章于 2023-07-04 10:33:22 发布

阅读量895

点赞数 1

分类专栏： c/c++ 文章标签： C

本文链接：https://blog.csdn.net/gm_ergou/article/details/118391200

版权

c/c++ 专栏收录该内容

3 篇文章 1 订阅

订阅专栏

由于算法要运行在手表上，算力和内存空间有限，需要进行内存压缩和c代码实现。

状态识别（计算步数+分段滤波）_传统处理方法

Status_detection.c

#include <stdio.h>
#include <stdbool.h> 
#include <math.h>
#include <stdlib.h>


#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]<peak2_index[0] && allPeak1_index[base_index1 - 1]>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[base_index2 - 1])
			{
				for (i = 1; i < 33; i++)
				{
					peak2_index[i - 1] = (i < peak2_nums) ? peak2_index[i] : 0;
				}
				peak2_nums = peak2_nums - 1;
			}

			for (i = 0; i < peak2_nums; i++)
			{
				allPeak2[base_index2 + i] = data[peak2_index[i]];
				allPeak2_index[base_index2 + i] = peak2_index[i] + 200 * nums;
			}
			allPeak2_nums[nums] = peak2_nums;
		}
	}

	// 4.删除开头和结尾的波峰点
	short peak1_total;
	short peak2_total;
	peak1_total = allPeak1_nums[0] + allPeak1_nums[1] + allPeak1_nums[2];
	if (allPeak1_index[0] < allPeak2_index[0])  // 开头
	{
		for (i = 1; i < 100; i++)
		{
			allPeak1_index[i - 1] = (i < peak1_total) ? allPeak1_index[i] : 0;
			allPeak1[i - 1] = (i < peak1_total) ? allPeak1[i] : 0;
		}
		allPeak1_nums[0] = allPeak1_nums[0] - 1;
	}
	peak1_total = allPeak1_nums[0] + allPeak1_nums[1] + allPeak1_nums[2];
	peak2_total = allPeak2_nums[0] + allPeak2_nums[1] + allPeak2_nums[2];
	if (allPeak1_index[peak1_total-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<stdio.h>
#include "opencv2/core/core.hpp"
#include "opencv2/ml/ml.hpp"
#include <cstdio>
#include <vector>
#include <iostream>

using namespace std;
using namespace cv;
using namespace cv::ml;


// n维vector转mat
Mat vector2Mat(vector<vector<float>> 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<float>(0, index) = src[i][j];
		}
	}
	return test;
}

// 数据输入为vector
int StateEstimate(vector<vector<float>> acc, vector<vector<float>> gyr, vector<float> ppg, cv::Ptr<cv::ml::RTrees> model)
{
	// 挑出测试数据
	vector<vector<float>> all_data;
	vector<float> 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<float>(0, 0));
	if (action==2)
	{
		return 2;
	}
	return 0;
}

int main() {

	 定义数据
	vector<vector<float>> acc;
	for (int i = 0; i < 200; i++)
	{
		vector<float>data;
		for (int j = 0; j < 3; j++)
		{
			data.push_back(i * 3 + j);
		}
		acc.push_back(data);
	}

	vector<vector<float>> gyr;
	for (int i = 0; i < 200; i++)
	{
		vector<float>data;
		for (int j = 0; j < 3; j++)
		{
			data.push_back(i * 3 + j);
		}
		gyr.push_back(data);
	}
	
	vector<float> ppg;
	for (int i = 0; i < 200; i++) {
		ppg.push_back(i);
	}

	// 模型加载
	cv::Ptr<cv::ml::RTrees> 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;

}