MPU6050卡尔曼滤波6轴 模拟鼠标 C# 代码

您好！感谢您的提问。以下是使用MPU6050卡尔曼滤波实现6轴模拟鼠标的C#代码示例：

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.IO.Ports;

public class MPU6050Mouse : MonoBehaviour
{
    SerialPort sp = new SerialPort("COM3", 9600); //定义串口
    float[] angle = new float[3];
    float[] gyro = new float[3];
    float[] accel = new float[3];
    float[] gyro_offset = new float[3];
    float[] accel_offset = new float[3];
    float[] angle_speed = new float[3];
    float[] angle_last = new float[3];
    float[] angle_now = new float[3];
    float[] angle_acc = new float[3];
    float[] angle_offset = new float[3];
    float[] Q = new float[3] { 0.001f, 0.001f, 0.001f };
    float[] R = new float[3] { 0.03f, 0.03f, 0.03f };
    float[] P = new float[3] { 0f, 0f, 0f };
    float[] K = new float[3] { 0f, 0f, 0f };
    float dt;
    float roll, pitch, yaw;
    float mouse_speed_x, mouse_speed_y;

    // Start is called before the first frame update
    void Start()
    {
        sp.Open(); //打开串口
        sp.ReadTimeout = 20; //设置读取超时时间
        gyro_offset[0] = 0;
        gyro_offset[1] = 0;
        gyro_offset[2] = 0;
        accel_offset[0] = 0;
        accel_offset[1] = 0;
        accel_offset[2] = 0;
        angle_last[0] = 0;
        angle_last[1] = 0;
        angle_last[2] = 0;
    }

    // Update is called once per frame
    void Update()
    {
        string str = sp.ReadLine(); //读取串口数据
        string[] data = str.Split(','); //分割数据
        gyro[0] = float.Parse(data[0]);
        gyro[1] = float.Parse(data[1]);
        gyro[2] = float.Parse(data[2]);
        accel[0] = float.Parse(data[3]);
        accel[1] = float.Parse(data[4]);
        accel[2] = float.Parse(data[5]);
        dt = Time.deltaTime;
        gyro[0] -= gyro_offset[0];
        gyro[1] -= gyro_offset[1];
        gyro[2] -= gyro_offset[2];
        if (Mathf.Abs(accel[0]) < 0.6f && Mathf.Abs(accel[1]) < 0.6f && Mathf.Abs(accel[2]) < 0.6f)
        {
            angle_acc[0] = Mathf.Atan(accel[1] / Mathf.Sqrt(Mathf.Pow(accel[0], 2) + Mathf.Pow(accel[2], 2))) * Mathf.Rad2Deg;
            angle_acc[1] = Mathf.Atan(-accel[0] / Mathf.Sqrt(Mathf.Pow(accel[1], 2) + Mathf.Pow(accel[2], 2))) * Mathf.Rad2Deg;
            angle_acc[2] = 0;
            angle_speed[0] = gyro[0] - angle_offset[0];
            angle_speed[1] = gyro[1] - angle_offset[1];
            angle_speed[2] = gyro[2] - angle_offset[2];
            for (int i = 0; i < 3; i++)
            {
                angle_now[i] = angle_last[i] + angle_speed[i] * dt;
                P[i] = P[i] + Q[i];
                K[i] = P[i] / (P[i] + R[i]);
                angle_now[i] = angle_now[i] + K[i] * (angle_acc[i] - angle_now[i]);
                P[i] = (1 - K[i]) * P[i];
                angle_last[i] = angle_now[i];
            }
            roll = angle_now[0];
            pitch = angle_now[1];
            yaw = angle_now[2];
            mouse_speed_x = pitch;
            mouse_speed_y = roll;
            Cursor.lockState = CursorLockMode.Locked;
            Cursor.visible = false;
            transform.Rotate(-mouse_speed_x, mouse_speed_y, 0); //旋转物体
        }
    }

    void OnApplicationQuit()
    {
        sp.Close(); //关闭串口
    }
}

以上代码实现了通过MPU6050卡尔曼滤波获取物体的旋转角度，从而模拟鼠标的移动。其中，gyro_offset和accel_offset是用来校准陀螺仪和加速度计的偏移量；Q、R、P和K是卡尔曼滤波中的参数；mouse_speed_x和mouse_speed_y是模拟鼠标的速度。希望对您有所帮助！